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ABSTRACT 


The  ability  of  an  Autonomous  Underwater  Vehicle  (AUV)  to  dynamically  plan  safe  routes  and 
maneuvers  in  dangerous  environments  is  directly  relevant  for  the  future  of  the  use  of  AUVs 
in  the  exploration  and  exploitation  of  the  underwater  environment,  specifically  the  littorals 
and  inland  waters.  This  thesis  builds  upon  the  existing  body  of  knowledge  of  the  REMUS 
AUV  dynamics  and  kinematics  and  develops  a  control  scheme  for  a  real-time  optimized  vehicle 
trajectory  that  will  permit  continuous  and  autonomous  collection  and  exploitation  of  external 
sensor  data,  which  will  facilitate  full  360-degree,  2-dimensional  mapping  of  the  underwater  en¬ 
vironment  surrounding  the  vehicle  while  preventing  the  vehicle  from  coming  into  contact  with 
mapped  objects  in  the  water.  The  developed  control  schema  will  seek  to  generate  a  trajectory  in 
real-time  that  optimizes  a  key  parameter  of  interest,  the  Information  Gain,  while  minimizing  a 
specified  cost  function  of  constraints,  such  as  kinematic  limits  and  obstacle  avoidance  criteria. 
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CHAPTER  1: 
Introduction 


The  ocean  is  a  vast  and  ever-changing  environment.  It  presents  significant  challenges  for  those 
that  seek  to  explore  and  exploit  it.  Much  of  it  is  unknown  and  unexplored,  and  even  much  of 
the  known  areas  can  be  inhospitable  and  downright  hostile  to  human  intrusion.  It  has  been  said 
that  we  know  more  about  outer  space  than  our  own  oceans  [1];  yet  it  remains  one  of  mankind’s 
most  vital  and  heavily  exploited  environments  [2].  Ships  have  plied  the  seas  for  exploration, 
merchant  trade,  and  military  operations  for  millennia,  but  until  the  last  century,  what  lie  below 
the  surface  of  the  sea  remained  in  the  realm  of  myth  and  fantasy.  While  merchant  mariners 
remain  largely  content  to  safely  ply  the  navigable  waters  on  the  surface  with  little  concern 
as  to  what  lies  below,  the  undersea  environment  is  of  great  interest  to  scientists,  engineers, 
and  military  planners  and  operators.  To  that  end,  great  strides  have  been  made  in  exploring 
this  undersea  world,  from  the  deepest  abysses  to  the  shallow  littorals  and  inland  waterways 
connected  therewith.  As  evidenced  by  recent  funding,  research  and  development  priorities,  the 
Navy  has  significant  interest  in  exploring  these  shallow  water  environments  [3]. 

1.1  Motivation 

Dominance  of  the  shallow  waters  of  the  littorals  and  inland  waters  is  a  vital  concern  for  Mar¬ 
itime  Component  Commanders.  Amphibious  operations,  Naval  Surface  Gunfire  Support  mis¬ 
sions,  Special  Operations,  submarine  strike  packages,  near-shore  and  inshore  reconnaissance, 
mine  countermeasures,  riverine  combat,  and  waterway  defense  are  just  a  few  of  the  Naval  op¬ 
erations  that  exploit  this  waterspace.  To  enable  such  dominance,  the  Commander  must  have 
the  tools  at  his  disposal  to  safely  and  completely  explore  unknown  areas,  investigate  and  map 
underwater  features,  and  locate  and  counter  submerged  mines.  Remotely  Operated  Vehicles 
(ROV’s)  and  Autonomous  Underwater  Vehicles  (AUVs)  are  being  used  extensively  in  fields  of 
underwater  cartography,  exploration,  salvage,  and  oceanographic  research  [4].  In  recent  years, 
the  Navy  has  recognized  their  extreme  utility  in  meeting  the  aforementioned  needs  in  the  context 
of  military  planning  and  operations.  Just  as  Unmanned  Aerial  Vehicles  have  seen  widespread 
use  in  similar  roles  by  Air  and  Ground  Commanders,  AUV  systems  are  rapidly  becoming  the 
go-to  tool  for  Maritime  Commanders  to  meet  their  unique  needs  in  the  littorals.  Some  of  the 
AUV’s  attributes  include: 
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Human  Safety  -Since  AUVs  are  unmanned,  the  risk  to  human  operators  is  reduced.  Addi¬ 
tionally,  they  can  be  used  in  roles  traditionally  performed  by  human  divers,  thus  further 
eliminating  the  risk  to  the  Navy’s  valuable  human  capital. 

Flexible  -Modem  AUVs  are  designed  and  constructed  with  modularity  in  mind,  allowing  them 
to  be  outfitted  with  sensors,  actuators,  and  tools  to  accomplish  a  wide  variety  of  missions. 

Mobile  -Many  AUV  systems  (vehicle  and  associated  support  equipment)  are  small  enough  to 
fit  into  the  bed  of  a  pickup  truck  or  onto  a  helicopter,  and  are  man-portable,  allowing  them 
to  be  quickly  delivered  just  about  anywhere  they  are  needed.  Additionally,  their  small  size 
allows  them  to  maneuver  in  spaces  or  areas  inaccessible  by  manned  underwater  vehicles. 

Autonomous  -By  definition,  AUVs  are  autonomous,  meaning  that  once  set  upon  it’s  mission, 
no  human  control  or  intervention  is  required  to  accomplish  the  mission,  resulting  in  lower 
manpower  requirements,  almost  zero  risk  of  danger  to  human  operators,  and  much  higher 
cost  savings. 

Low  Cost  -Cost  savings  over  traditional  means  for  accomplishing  the  stated  objectives  are 
reached  by  use  of  Commercial  Off-The-Shelf  (COTS)  technologies,  reduction  of  man¬ 
power  requirements,  size,  and  modularity.  Further,  the  minimal  amount  of  support  equip¬ 
ment  required  for  many  AUVs  is  far  less  than  that  of  a  comparably  capable  ROV,  further 
reducing  costs. 

1.2  Problem  Statement 

The  Navy  is  interested  in  making  AUVs  more  autonomous.  In  general,  AUV  autonomy  is 

defined  by  the  ability  of  the  vehicle  to  interpret  it’s  environment  and  plan  trajectories  that  best 

adhere  to  the  mission  objectives.  It  is  characterized  by  four  distinct  aspects: 

Internal  Representation  of  the  Environment  -  the  spatial  mapping,  or  "Environmental  Map", 
of  objects  and  obstacles  in  the  environment  in  which  the  vehicle  is  operating 

Path  Planning  -  creating  a  path  to  follow;  two  types  of  path  planning:  deliberative,  where 
the  path  is  planned  based  on  known  information  stored  in  the  Environmental  Map;  and 
reactive,  where  the  vehicle  detects  objects  or  obstacles  not  already  existing  in  the  Envi¬ 
ronmental  Map  and  plans  a  path  to  avoid  them 

Path  Following  -  Path  planning  results  in  a  trajectory  that  the  vehicle  is  trying  to  follow.  This 
provides  direction  to  the  controls,  which  then  drives  the  vehicle  along  the  path. 

Sensory  processing  -As  new  sensory  information  is  obtained,  such  as  position  and  other  raw 
sensory  data,  it  is  fed  back  into  the  loop  to  localize  the  vehicle  and  update  the  World  map. 
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Reactive  obstacle  detection  is  an  example  of  a  behavior  exhibiting  greater  autonomy.  The  ex¬ 
ternal  representation  of  the  AUV  environment  could  be  a  map  with  the  water  depths  and  known 
features  and  obstacles.  The  path  planning  module  uses  this  information  to  plan  a  path.  This 
path  must  be  calculated  by  considering  the  limitations  in  the  AUVs  maneuverability.  Finally, 
sensors  such  as  a  forward  looking  sonar  provide  imagery  regarding  the  environment  forward 
of  the  AUV.  Computer  vision  techniques  are  used  on  the  images  and  pertinent  information  is 
extracted  and  entered  into  the  map.  This  feedback  loop  continues  through  the  entire  deployment 
cycle  and  must  be  accomplished  in  near  real-time.  [5] 

For  this  thesis,  I  am  interested  persistent  AUV  autonomy.  This  is  ability  of  the  AUV  to  remain 
deployed  for  longer  periods  of  time.  The  specific  concept  of  operations  is  to  put  the  vehicle 
into  a  suspended  sleep  state.  All  primary  systems  are  shut  down  to  conserve  battery  life  with 
the  exception  of  a  communications  or  sensing  device  and  the  computer  to  run  them.  Before  the 
AUV  goes  into  this  sleep  mode,  it  surveys  the  area.  During  the  survey  it  is  looking  for  objects 
readily  detected  by  the  forward  looking  sonar. 

The  AUV  is  eventually  awakened  via  communications  or  sensory  detection.  The  first  objec¬ 
tive  is  to  local  i/e  the  position  of  the  AUV.  Due  to  limitations  in  positive  ballast  and  oceanic 
conditions,  the  AUV  may  have  drifted  along  the  ocean  floor.  For  tactical  and  safety  reasons, 
it  is  preferable  not  to  surface  the  AUV  for  a  GPS  fix.  The  goal  is  for  the  AUV  to  efficiently 
search  the  area  for  the  navigational  fixes  that  were  surveyed  prior  to  the  sleep  mode.  This  thesis 
addresses  the  path  planning  necessary  for  such  a  behavior.  The  goal  is  to  develop  an  informa¬ 
tion  theoretic  search  plan  for  the  AUV  to  detect  the  survey  sites  taking  into  consideration  the 
uncertainty  associated  with  vehicle  position  due  to  drift. 

This  thesis  describes  an  optimal  two-dimensional  exploratory  path-planning  schema  for  the 
REMUS  AUV1  that  utilizes  well  known  and  understood  concepts  of  probabilistic  analysis 
(Bayesian  inference)  and  information  theoretics  (entropic  information,  divergence  measures, 
etc.)  for  analyzing  and  quantifying  the  information  gain  achieved  by  an  onboard  organic  sensor. 
It  is  this  latter  subject  of  information  analysis  that  is  of  key  importance  to  this  thesis.  Rigorous 
analysis  will  be  given  to  determining  the  information  gain  achieved  by  the  autonomous  system 
in  the  course  of  following  various  candidate  trajectories.  Information  gain,  in  the  context  of  the 

'Although  the  methodologies  explored  by  this  thesis  have  been  developed  for  use  on  the  REMUS  AUV,  they 
are  obviously  not  limited  to  use  on  this  vehicle,  and  may  certainly  be  more  generally  applied,  with  minimal  effort 
or  modification,  to  use  in  path  planning  for  practically  any  autonomous  agent  operating  under  similar  conditions 
and  constraints. 
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present  problem  being  addressed,  represents  a  quantitative  and  qualitative  measure  of  the  situa¬ 
tional  awareness  and  utility  of  the  data  models  being  collected  and  developed  within  the  system. 
This  information  gain  will  be  utilized  as  the  primary  optimized  parameter  in  the  exploratory  path 
planning  and  optimization  routine.  For  this,  the  author  will  employ  a  contemporary  approach 
to  trajectory  generation  and  optimization  known  as  Direct  Method  of  the  Inverse  Dynamics  in 
the  Virtual  Domain  (hereafter  referred  to  as  DM-IDVD),  that  not  only  ensures  complete  ex¬ 
ploratory  coverage  of  the  unknown  underwater  space  in  which  the  vehicle  operates,  but  allows 
real-time,  near-optimal  trajectory  generation  within  the  performance  constraints  of  off  the  shelf 
computing  and  control  systems  widely  used  on  autonomous  vehicles. 

1.3  Literature  Review 

Path  planning  is  one  of  the  most  rigorously  studied  problems  in  the  field  of  robotics.  Numerous 
techniques,  methodologies,  and  algorithms  exist  to  process  sensor  information  and  plan  a  tra¬ 
jectory  to  direct  an  agent  (e.g.  vehicle,  robotic  device,  etc.)  from  an  initial  state  to  a  final  state 
subject  to  dynamic,  kinematic,  environmental,  configurational,  or  other  constraints.  The  algo¬ 
rithms  and  guidance  laws  produced  by  such  methodologies  address  such  issues  as  localization, 
track  following,  cross-track  error  control,  obstacle  avoidance,  and  trajectory  optimization.  Of 
key  importance  to  the  efficacy  of  almost  all  of  these  methodologies,  as  applied  to  an  autonomous 
vehicle,  is  the  means  by  which  the  information  gathered  by  the  agent’s  onboard  sensor(s)  is  col¬ 
lected,  measured,  quantified,  analyzed,  and  applied. 

1.3.1  Trajectory  Generation 

In  the  context  of  the  REMUS  AUV,  many  path  planning  methods  have  been  explored.  Most 
earlier  works  focused  on  simplified  path  planning  problems  such  as  contour  following  and  ob¬ 
stacle  avoidance.  Van  Reet  explored  contour  following  techniques  utilizing  logic-based  gra¬ 
dient  methods  [6].  Heminger  [7]  and  Healey  [8]  developed  reactive  obstacle  avoidance  tech¬ 
niques  which  used  Gaussian  Potential  Functions  for  obstacle  avoidance.  Furukawa  worked  on 
an  obstacle  avoidance  routine  that  combined  a  spline  addition  planner  with  a  look-ahead  pitch 
controller  [9].  While  sufficient  for  simplified  mission  tasks,  these  approaches  have  intrinsic 
limitations  and  drawbacks  when  applied  to  more  complex  scenarios,  including  sub-optimality, 
computational  intensity,  ambiguity  in  situational  awareness,  and  sensitivity  to  error. 

The  Direct  Methods  approach  seeks  to  mitigate  these  issues,  and  of  late,  much  attention  here  at 
NPS  has  been  given  to  applying  this  technique  to  autonomous  system  navigation.  Recent  work 
includes  that  of  Yakimenko,  Homer,  Pratt,  and  Kragelund.  Horner  and  Yakimenko  were  the 
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first  to  incorporate  Direct  Methods  into  the  obstacle  avoidance  path  planning  framework.  Their 
2007  paper  [5]  utilized  sensor  data  in  a  closed  feedback  loop  and  a  known  environmental  map 
to  provide  the  situational  awareness  to  the  DM-IDVD  algorithm.  The  algorithm  then  generated 
a  near-optimal  trajectory  for  navigation  and  obstacle  avoidance,  updating  in  real  time  along  the 
vehicle’s  path.  Yakimenko  then  expanded  on  this  paper  in  2008  [10],  incorporating  the  DM- 
IDVD  method  into  a  formulation  of  a  more  generalized  approach  to  real-time,  near-optimal, 
obstacle  avoidance,  3D  spatial  trajectories.  Furthering  the  research  into  applying  the  DM-IDVD 
approach  to  AUV  navigation  was  the  paper  by  Yakimenko,  Homer  and  Pratt  which  detailed  a 
control  algorithm  for  autonomously  deploying  and  recovering  AUVs  [11].  In  this  paper,  the 
authors  apply  the  DM-IDVD  method  to  calculating  trajectory  of  an  AUV  out  of  (deployment) 
or  into  (recovery)  a  submerged  docking  apparatus,  reducing  the  AUVs  dependency  on  the  host 
surface  vessel  or  swimmers.  What  each  of  these  approaches  had  in  common  is  their  use  of 
known  a  priori  information  of  the  environment  for  localization  and  navigation.  The  DM-IDVD 
method  was  applied,  more  or  less,  to  the  task  of  obstacle  avoidance  or  waypoint  navigation. 

1.3.2  Sensor  Information  Processing 

As  alluded  to  previously,  the  methods  and  techniques  by  which  sensor  data  is  analyzed  and 
applied  is  absolutely  key  to  the  efficacy  of  any  path  planning  algorithm.  The  methodology  ex¬ 
plored  by  this  thesis  utilizes  existing  techniques  for  handling  a  sonar  image  input  and  converting 
that  image  into  a  probabilistic  model  of  the  environment.  Extensive  use  is  made  of  McChes- 
ney’s  work  [12]  with  manipulating  the  sonar  image  collected  by  the  REMUS  vehicle’s  onboard 
blazed  array  sonar  sensor2.  In  his  thesis,  McChesney  processes  a  three-dimensional  sonar  im¬ 
age  by  first  developing  noise  and  signal  confidence  probability  models  of  the  sonar  sensor  itself, 
and  then  applies  a  probabilistic  update  process  to  develop  a  three  dimensional  spatial  model  of 
the  underwater  environment.  This  spatial  model  is  constructed  by  way  of  an  Occupancy  Grid 
(OG).  The  Bayesian  probabilistic  methods  used  to  build  and  update  the  OG  have  been  rigor¬ 
ously  explored  and  presented  by  Elfes  [13]  and  Noykov  [14].  The  exact  method  whereby  the 
collected  sonar  image  is  used  to  develop  the  OG  will  be  discussed  in  detail  in  Chapter  2. 

In  this  thesis,  information  gain  (IG)  is  the  key  informational  parameter  under  consideration. 
To  that  end,  it  must  be  clarified  that  the  information  stored  in  the  OG  is  merely  a  means  to 
an  end.  As  our  primary  concern  is  with  determining  how  much  information  is  gained  by  the 

2While  the  McChesney  thesis  deals  with  feature  reconstruction  in  three  dimensions  using  both  horizontal  and 
vertical  elements  of  the  Blue  View  sonar  package  installed  onboard  the  REMUS  vehicle,  this  thesis  will  work  in 
two  dimensions  only,  dealing  with  the  horizontal  plane. 
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system  for  a  given  candidate  trajectory,  a  means  must  be  utilized  with  which  we  can  analyze  the 
gain,  or  divergence,  in  cumulative  information  between  discrete  consecutive  updates  of  the  OG. 
The  tools  used  in  the  methodology  explored  by  this  thesis  come  from  the  field  of  Information 
Theoretics.  Much  use  is  made  of  the  concepts  of  divergence  measures,  entropic  information, 
and  Fisher  information,  as  presented  in  the  context  of  autonomous  agent  path  planning  in  the 
Levine  thesis  [15].  Although  Levine  applies  these  concepts  to  a  very  different  approach  to  path 
planning  (Rapidly-exploring  Random  Trees),  they  are  equally  apropos  for  the  approach  used  in 
this  thesis. 

1.4  Thesis  Organization 

Chapter  2  of  this  thesis  will  discuss  the  generalized  information  theoretics  and  probabilistic 
methods  that  will  be  used  to  process,  update,  and  quantify  the  sonar-sensor  information.  I 
will  first  describe  the  conditional  probabilistic  process  of  Bayesian  inference.  This  process 
will  then  be  applied  to  the  formulation  of  the  Occupancy  Grid  (OG)  and  the  update  process. 
Next,  the  critical  foundational  concepts  of  information  theoretic  measures  will  be  discussed.  It 
will  cover  entropic  information  (conditional  entropy),  divergence  measures  (Kullback-Leibler 
divergence),  information  gain,  and  Fisher  information  matrices. 

Chapter  3  provides  a  generalized  discussion  of  the  Direct  Methods  approach.  It  is  the  path 
planning  methodology  used  for  the  thesis.  Chapter  4  will  tie  the  concepts  and  methods  discussed 
in  the  previous  two  chapters  into  a  complete  path  planning  routine.  First,  the  methods  and 
techniques  by  which  the  simulated  sonar  image  is  processed  into  an  environmental  spatial  model 
is  discussed.  Then  the  process  by  which  the  probabilistic  measures  of  entropy  and  information 
gain  are  quantified  are  elaborated  upon.  Next,  the  constraints  on  the  states,  controls,  and  their 
derivatives  will  be  defined.  Optimization  parameters  and  the  concomitant  cost  functions  which 
incorporate  the  previously  discussed  information  metrics  will  then  be  developed.  The  candidate 
trajectory  reference  functions  will  be  presented,  followed  by  a  discussion  of  parameterization 
of  the  reference  functions  in  the  virtual  domain.  Initial  and  final  conditions  on  the  fixed  and 
variable  parameters  of  the  reference  functions  will  then  be  established.  The  inverse  dynamic 
equations  are  then  developed.  Finally,  with  all  the  pieces  in  place,  the  optimization  routine 
will  be  discussed  and  analyzed.  The  remaining  chapters  cover  the  analysis  of  the  results,  the 
findings  therein,  and  finally,  the  conclusion  including  a  brief  discussion  of  opportunities  for 
further  research  relevant  to  this  thesis  topic. 
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CHAPTER  2: 
Information  Theoretics 


As  stated  in  Chapter  1,  the  ultimate  goal  of  the  approach  developed  by  this  thesis  is  to  produce 
an  optimal  trajectory  that  maximizes  the  information  gain  achieved  by  that  trajectory.  Before 
examining  the  Direct  Methods  process  in  detail  and  describing  the  approach  developed  by  the 
author,  a  primer  on  some  of  the  basic  information  theoretic  measures  employed  in  the  author’s 
method.  In  this  chapter,  a  few  of  these  measures  will  be  reviewed.  Bayesian  inference  is  a 
key  component  of  the  probabilistic  analysis  performed  during  the  analysis  and  quantification  of 
the  received  sonar  image  and  the  construction  of  the  corresponding  occupancy  grid.  Entropic 
information  is  an  important  metric  in  determining  the  value  of  the  information  received  by  the 
system.  Divergence  measures,  such  as  information  gain,  enable  the  quantification  of  increase  in 
knowledge  gained  during  the  process,  and  as  stated  before,  are  key  parameters  for  optimization. 

2.1  Probabilistic  Measures 

2.1.1  Basic  Probability  Concepts 

Before  discussing  Bayesian  inference  itself,  some  basic  probabilistic  concepts  must  be  eluci¬ 
dated.  Let  consider  a  random  variable  X,  and  let  x  denote  any  specific  value  that  X  may  be.  For 
example,  if  we  consider  a  simple  coin  flip,  where  X  is  the  random  variable  that  represents  the 
side  of  the  coin  that  turns  up  from  the  flip,  then  the  possible  values  of  x  are  heads  or  tails.  The 
probability  that  X  has  a  value  of  .r  is  given  by 


P(X=x),  (2.1) 

and  is  typically  shortened  to  px(x),  or  simply  p(x).  So  for  the  example  of  the  coin  flip,  the 
probability  that  heads  turns  up  is  indicated  by  p(X  =  heads),  and  the  probability  of  tails  turning 
up  is  p(X  =  tails).  For  a  “fair"  coin,  where  there  is  equal  probability  of  achieving  either  result, 
p(X  =  heads)  —  p(X  =  tails)  —  Furthermore,  discrete  probabilities  always  sum  to  unity, 
thus  T,xp(X  =x)  =  1  [16]. 

A  joint  distribution  of  X  and  Y  is  one  in  which  p(x.y)  =  p(X  =  x  and  Y  =  y),  which  describes 
the  probability  of  the  event  where  X  —  x  and  Y  —  y  simultaneously.  If  neither  random  variable 
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depends  on  the  other  for  their  respective  values,  we  call  the  random  variables  independent ,  such 
that  p(x.y)  =  p(x)p(y). 


Conditional  probabilities  are  another  form  of  probabilistic  measure  that  give  us  information 
about  the  likely  value  of  one  random  variable  given  known  information  about  another.  In  other 
words,  a  conditional  probability  is  the  probability  that  random  variable  X  takes  on  a  value  of  x 
given  that  we  know  another  random  variable  Y  has  a  value  of  y.  Such  probabilities  are  usually 
denoted 

p(X=x\Y=y),  (2.2) 


or  more  commonly 

Px\r{Ay)=P(x\y)-  (2-3) 

This  is  a  key  fundamental  concept,  as  it  will  be  heavily  utilized  in  the  Bayesian  inference  process 
to  be  discussed  in  the  next  section.  Furthermore,  if  p(y)  >  0,  then 


p(x\y) 


p{x,y) 

piy) 


and  if  X  and  Y  are  independent  as  defined  above,  we  get 


p(x\y) 


p(x)p(y) 

piy) 


p(x). 


(2.4) 


(2.5) 


This  then  leads  to  one  last  concept  relevant  to  Bayesian  inference,  the  Law  of  Total  Probability. 
If  X  and  Y  are  independent,  and  Y  is  a  set  of  mutually  exclusive  and  exhaustive  events  (meaning 
only  one  event  can  occur  at  a  time,  and  all  possible  events  are  contained  in  the  set),  then  for  any 
other  event  X, 


p(x)  =  p(x\yi)p(yi)  +  p(x\y2)p(y2)  +  ■  ■  ■  +  pix\yn)p(yn)  =  Y,P(x\y)p(y)  (2-6) 

y 

in  the  discrete  case.  As  we  will  see  in  the  next  section,  this  law  forms  the  denominator  of  the 
Bayesian  Inference  equation. 

2.1.2  Bayesian  Inference 

Bayesian  inference  is  the  core  concept  of  Bayesian  decision  theory  in  which  information  re¬ 
garding  the  current  state  of  a  random  variable  is  inferred  from  prior  knowledge  of  states  of  that 
random  variable  and  present  observations.  It  allows  us  to  iteratively  update  the  specific  current, 
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or  posterior,  probability  distribution  of  a  random  variable  based  on  observational  conditional 
probabilities  of  certain  events,  and  knowledge  of  the  previous,  or  prior  probability  distribution. 
Bayesian  inference  is  defined  by  Bayes  rule.  Let  X  be  a  random  variable  with  possible  value,  or 
state,  x  drawn  randomly  from  a  set  of  possible  values,  %.  Before  any  observations  are  made,  the 
knowledge  of  x  is  given  entirely  by  the  prior  probability  distribution  p(x).  We  model  observa¬ 
tions  of  the  system  as  random  variable  Y  that  take  on  values  of  y.  Given  such  observations  (e.g. 
sensor  data),  we  may  infer  the  value  of  x  from  that  of  y  by  way  of  the  Bayes  rule  equation: 


/  I  \  _  P(y\x)p(x)  _  p(y\x)p(x) 

P[ly)  piy)  J>(y|*W) 

x! 


(2.7) 


Here,  p{x\y)  is  called  the  posterior  probability  distribution  and  p{y\x)  is  called  the  inverse 
conditional  probability.  This  latter  component  essentially  describes  the  probability  of  receiving 
value  y  given  a  specific  value  of  x,  for  example,  the  likelihood  that  a  specific  state  x  value, 
represented  here  by  x' ,  causes  a  specific  sensor  measurement  value  of  y. 


2.2  Entropy  and  Information  Gain 

As  previously  stated,  the  objective  of  this  thesis  is  to  plan  a  trajectory  that  optimizes  the  in¬ 
formation  gain  achieved  by  that  trajectory.  In  order  to  quantify  this  information  gain,  we  must 
first  describe  the  measure  that  it  is  predicated  on,  the  information  entropy.  In  the  context  of 
information  theoretic  measures,  the  entropy  of  a  random  variable  X,  denoted  H,  is  essentially  a 
measure  of  the  amount  of  uncertainty  associated  with  the  values  of  X.  It  is  the  expectation  of 
the  information  that  each  value  of  the  random  variable  carries,  and  is  defined  mathematically 
by  equation  (2.8)  [17].  If  X  can  take  on  any  value  x  from  set  of  possible  values  %,  and  p(x)  is 
the  probability  of  X  assuming  value  x,  then  the  entropy  of  X  is  given  as 

H(X)  -  E* [/(*)]  =  -  £  p(x)logp(x).  (2.8) 

-vex 

where  I(x)  is  the  self  information  of  x,  given  by  I(x)  —  log  p(x),  which  represents  the  entropy 
contribution  carried  by  each  value  of  x.  [17] 

In  the  special  case  where  random  variable  X  can  only  take  on  two  possible  values,  such  as  the 
case  in  the  coin  flip  example,  the  entropy  function  simplifies  to  the  binary  entropy  function 
given  by  (2.9)  [17]: 

Hb  =  —p  log2  p  —  (l—  p)  log2  p)  (2.9) 
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Figure  2.1:  Binary  Entropy 

This  function  is  plotted  in  Figure  2.1.  In  this  figure,  we  can  see  that  the  maximum  entropy 
occurs  where  the  probability  that  X  —  1  (P(X  —  1))  is  0.5.  (Note  that  since  this  is  a  binary 
function,  where  X  can  only  equal  1  or  0,  then  if  P(X  =  1)  =  0.5,  then  P{X  —  0)  =  0.5.)  This 
probability  value  for  maximum  binary  entropy  will  be  used  to  initialize  the  occupancy  grid 
discussed  in  Section  2.3. 

Information  Gain ,  also  known  as  Kullback-Leibler  divergence ,  is  a  measure  of  the  difference 
between  two  probability  distributions:  a  “true"  probability  distribution  p(X),  and  an  “assumed" 
distribution,  q(X).  If  we  initially  assume  that  the  distribution  of  values  x  of  random  variable 
X  is  given  by  q(X),  when  the  actual  correct  distribution  is  p{X).  then  the  difference  measure 
between  the  two  distributions  is  given  by  the  Kullback-Leibler  divergence,  defined  as 

DKL(p(x)\\q(x))  =  Y  -p(x)\o gq(x)  -  (-p(x)\ogp(x))  =  Y  P(x)loS^rl  (2.10) 

-rex  Jtex  CPX) 

If  q(x)  represents  our  prior  belief  of  the  probability  that  X  —  x,  and  subsequent  observations 
(or  measurements)  reveal  a  true,  or  correct,  probability  distribution  p(x),  then  the  information 
gain  measures  how  far  from  the  true  distribution  our  initial  belief  was.  Thus  by  observing  and 
measuring  a  probabilistic  quantity  (i.e.  random  variable)  and  updating  the  probability  distribu¬ 
tion,  then  those  observations  carry  information  about  that  quantity  causing  us  to  increase  our 
knowledge  about  that  quantity,  and  this  gain  is  quantified  as  the  information  gain. 

2.3  Occupancy  Grids 

As  robotic  sensors  are  never  perfectly  accurate,  and  are  thus  typically  modeled  probabilistically, 
a  means  for  generating  a  high-confidence  spatial  map  of  the  sensed  environment  based  on  this 
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Table  2.1:  Occupancy  Grid  Equation  Terms 


POcc 

P[s{C)  =  Occ\{r}t+x] 

Probability  that  a  cell  is  occupied  given 
the  current  and  all  previous  measurements 

PI 

P[v(C)  =  Occ\{r}t] 

Probability  that  a  cell  is  occupied  given 
all  previous  measurements 

P2 

P[v(C)  =Emp\{r}t] 

Probability  that  a  cell  is  empty  given  all 
previous  measurements 

P3 

P[rt+i\s(C)  =  Occ] 

Probability  of  receiving  the  current  mea¬ 
surement  given  that  the  cell  is  occupied 

P4 

P[rt+i\s(C)  =  Emp] 

Probability  of  receiving  the  current  mea¬ 
surement  given  that  the  cell  is  empty 

sensor  data  must  be  developed  based  on  probabilistic  concepts.  Elfes  developed  an  iterative 
method  based  on  Bayesian  inference  that  discretizes  the  spatial  map  into  a  grid  of  cells.  The 
goal  is  to  determine  whether  each  cell  is  occupied  or  empty.  The  occupancy  probability  of  each 
cell  given  all  previous  measurements  is  then  given  by 


P[v(C)  =  Occ\{r}t+1] 


P[rt+i\s(C)  =  Occ]  vP[s(C)  =  Occ\{r}t\ 

£  P[rm|5(C)]-P[5(C)|{r}r 

Vs(C) 


(2.11) 


where 


rt+ 1  :  The  current  measurement 

5(C)  :  State  of  the  cell  ([Occ]upied  or  [Emp]ty) 

{r}t :  All  measurements  up  to  time  t 


Expanding  the  summation  in  the  denominator,  and  subbing  in  short  variable  names  from  column 
1  of  Table  2.1,  the  Bayesian  update  equation  for  the  occupancy  grid  cells  then  becomes 


Pqcc 


P3  PI 

P4P2  +  P3PI 


(2.12) 


From  (2.12),  we  can  see  that  we  initially  have  4  unknown  variables  that  we  must  account  for. 
These  unknowns  are  summarized  in  Table  2.1  [12].  However,  recall  that  the  states  may  only 
have  two  possible  states,  Occupied  or  Empty,  soP[5(C)  =  Occ\{r}t]  +P[5(C)  =  Emp\{r}t\  —  1. 
So  we  can  substitute  1  —  P[s(C)  =  Occ\{r}t\  for  P[s(C)  =  Emp\{r}t\,  thereby  reducing  the 
number  of  variables  required  to  be  calculated  for  each  cell  to  three.  The  last  two  components  of 
Table  2.1  are  predicated  on  the  probability  distributions  of  the  sensor  model  employed.  These 
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Figure  2.2:  Sensor  Model  Probability  Distribution  Functions 

sensor  models  probability  distributions  are  typically  determined  experimentally.  An  example 
of  the  probability  distributions  for  the  sonar  sensor  employed  on  the  NPS  REMUS  vehicle 
can  be  seen  in  Figure  2.2.  The  second  and  third  components  represent  the  prior  probability 
distributions  of  occupancy  data  already  existing  in  the  occupancy  grid  for  each  cell. 

Equation  (2.12)  represents  the  iterative  process  that  occurs  for  each  cell  during  each  subsequent 
collection  of  sensor  data.  Thus  an  initial  value  is  required  for  the  occupancy  probability  of  each 
cell.  Since  it  is  assumed  that  at  time  t  —  0  we  know  nothing  of  the  occupancy  probability,  a  value 
that  represents  this  lack  of  knowledge,  or  information  is  needed.  Recalling  from  Section  2.2  that 
this  lack  of  knowledge  can  be  quantified  by  the  informational  entropy ,  we  initialize  all  cells  of 
the  OG  to  a  value  corresponding  to  maximum  entropy.  As  there  are  only  two  possible  values  that 
the  cells  can  take,  both  being  exclusive  and  exhaustive,  and  since  the  unconditional  probabilities 
of  the  cell  taking  each  of  these  values  are  equal,  then  the  corresponding  maximum  entropy  is 
0.5  (see  Section  2.2). 

To  illustrate  the  process,  consider  an  example  in  which  a  cell  begins  with  an  unknown  occu¬ 
pancy  state.  The  prior  probability  that  the  cell  is  occupied,  P[,s(C)  =  Occ\{r}t],  is  0.6,  and 
P[s(C)  =  Emp\{r}t\  —  1  —  P[s(C)  =  Occ\{r}t\  —  0.4.  Now  a  sonar  sweep  across  this  cell  re¬ 
ceives  a  return  signal,  rt+\,  with  a  value  of  3000.  From  the  sensor  probability  density  functions 
in  Figure  4.7,  the  corresponding  values  for  P[rt+\ |,s(C)  =  Occ]  and  P[rt+\\s(C)  —  Emp ]  are 
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approximately  0.01  and  0.001  respectively.  Now,  substituting  these  values  into  (2.12): 


P[S(C)  =  Occ\{r}t+l}=P0cc  = 


0.01-0.6 

0.001 -(1-0.6) +0.01 -0.6 


0.9375 


we  achieve  the  posterior  occupancy  probability  of  the  cell  conditioned  on  the  value  of  the  sonar 
signal  received,  which  in  this  case  is  equal  to  0.9375.  Each  cell  is  then  updated  in  the  same 
manner  using  the  prior  occupancy  probabilities  and  updated  with  the  sensor  model  probability 
values  based  on  new  sensor  measurements  for  each  sweep. 
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CHAPTER  3: 

Direct  Method  of  Inverse  Dynamics  in  the  Virtual 

Domain 


This  chapter  describes  a  method,  known  as  the  Direct  Method  of  Inverse  Dynamics  in  the  Vir¬ 
tual  Domain  (DM-IDVD),  whereby  a  set  of  candidate  spatial  trajectories  for  an  autonomous 
agent  is  generated  in  real-time  for  short-term  maneuvers  of  the  agent.  The  trajectories  are  gen¬ 
erated  by  way  of  chosen  reference  functions  that  consist  of  linear  functions  of  the  parameters  of 
the  system.  They  are  then  subjected  to  an  optimization  routine  that  seeks  to  minimize  a  given 
cost  function  by  way  of  built  in  or  predeveloped  minimization  computational  routines  or  algo¬ 
rithm,  such  as  fminsecirch  MATLAB  function,  or  the  Hookes-Jeeves  minimization  algorithm. 
The  DM-IDVD  process  results  in  the  efficient  solution  of  a  two-point  boundary  value  problem 
representing  the  trajectory  that  is  optimal  in  both  the  chosen  optimization  parameter  and  the  set 
of  constraints  based  on  vehicle  kinematics,  obstacle  avoidance  criteria,  etc.  The  efficiency  of 
the  routine  allows  for  real-time  trajectory  optimization  within  the  computational  capabilities  of 
most  modem  autonomous  systems.  [18] 

3.1  Problem  Formulation 

The  DM-IDVD  process  begins  by  assuming  that  there  exists  a  set  of  admissible  trajectories 
described  by  the  state  vector  z (t): 


Z (t)  =  [z\(t),z2{t),...,zr{t)]T  e  S 

S  =  {z(t)  GZrc£'},  t  e  [t(h  tf\ 

All  trajectories  in  the  set  must  satisfy 

1 .  the  system  of  ordinary  differential  equations  (typically  defined  by  the  vehicle  kinematics): 

Zi  =  fi(t,z,  u,c),  i  —  1,2, . . . ,  r 

where  u  is  the  vector  of  controls  given  by  u(f)  =  {ui(t),U2(t),...,ui(t)}T ,  l  <  r,  u  G  U1  C 
E1,  and  c  is  the  vector  of  vehicle  technical  characteristics  given  by  c  =  (ci,C2, ..., cp }T , 
c  G  Cp  C  Ep; 
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2.  initial  and  final  conditions  on  the  states  and  controls3: 


z(t0)=z0eS0,  Mzo  G  c  E’  }  (3.1) 

u(t0)  =  u0eRo,  Ro  =  {uo  e  Ul  C  E1}  (3.2) 

z(tf)  =  zf  e  Sf,  Sf{Zf  eZrc  Er}  (3.3) 

u(tf)  =  u/  e  Rf,  Rf  =  {uf<EUlcE1}  (3.4) 

3.  constraints  on: 

(a)  state  space: 

V(t,z)  =  {Vi(t,z),ri2(t,z),...,Tlw(t,z)}T  >0  (3.5) 

(b)  controls: 

£(t,z, u)  -  >0  (3.6) 

(c)  and  their  derivatives: 

7T(f,Z,U,u)  =  {Ki(t,Z,U,u),K2(t,Z,U,u),...,7la(t,Z,U,u)}T  >  0  (3.7) 

Given  the  set  of  trajectories  satisfying  these  requirements,  the  objective  then  is  to  determine 
the  best  (near-optimal)  trajectory  zopt(t )  from  within  the  set  and  that  trajectory’s  corresponding 
controls  u opt(t)  that  minimize  some  cost  function  J.  [18] 

3.2  Trajectory  Generation 

The  next  step  in  the  Direct  Methods  process  is  to  express  the  candidate  trajectories  of  the  states 
as  a  differentially  flat  function  of  some  abstract  parameter  t.  (This  virtual  parameter,  typi¬ 
cally  referred  to  as  the  virtual  arc,  will  be  discussed  in  more  detail  in  Section  3.3.)  Since 
the  main  idea  of  direct  methods  is  to  consider  the  solution  as  a  finite  set  of  variables  (func¬ 
tions)  [18],  we  first  assume  that  the  admissible  functions  can  be  expressed  as  an  infinite  power 
series  Zi( t)  =  E^=0 a  Fourier  series  Zi( t)  =  c/,o/2  +  Y^k^\  («/a-cos/cT  +  /?,/(sin/:T),  or  more 
generally,  by  any  series  function  of  the  form  z;(t)  —  Er=i  aik(Pik(T)  where  t)  is  any  suit¬ 
able  basis  function.  In  fact,  the  chosen  reference  functions  may  be  any  combination  of  basis 
functions  (e.g.  monomials,  trigonometric  functions,  etc.)  so  long  as  the  set  of  trajectories  gen¬ 
erated  by  them  meet  the  requirements  in  the  preceding  section.  Direct  methods  then  simplify 

3Although  this  definition  declares  the  terminal  parameters  as  completely  defined,  in  reality,  they  may  not  be. 
In  this  case,  these  "free  variables"  may  be  added  to  the  set  of  optimization  parameters  (OP’s). 
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the  problem  by  considering  the  solutions  to  be  functions  of  finite  series  vice  infinite.  The  solu¬ 
tion  in  this  case  is  then  merely  a  function  of  a  set  of  unknown  coefficients.  That  is  to  say  that 
Zi( t)  =  f(an). a,\ . . . .  .ajm.bj0.bi\ . . . .  ,bm,  t),  where  i  is  the  state  parameter  index  and  m  and  n 
are  the  orders  of  the  basis  functions. 

Consider,  for  instance  the  general  3D  case  where  the  coordinate  state  vector  for  an  autonomous 
agent  is  given  by  x  =  [a(t),)'(t),z(t)].  The  candidate  trajectories  of  the  agent  can  be  expressed 
as  a  polynomial  of  degree  N: 

N 

Xi(T)  =  Pi(T)  =  £  aikZk  (3.8) 

k= 0 

where  x\(x)  =  jc(t),jc2(t)  =  y(z),  and  JC3  (t)  =  z(  t).  The  degree  N  of  the  polynomial  is  depen¬ 
dent  on  the  number  of  boundary  conditions  that  must  be  satisfied  so  that  all  coefficients  are 
determined  algebraically  instead  of  being  varied.  The  variable  T  is  left  as  a  varied  parameter. 
In  general,  the  minimum  order  n  of  polynomial  required  is  determined  by  the  orders  of  the  time 
derivatives  of  the  initial  and  final  coordinates  (do  and  df  respectively): 

n  =  d0  T  df  T  1  (3.9) 

Boundary  conditions  at  the  initial  and  final  (terminal)  points  of  the  desired  trajectory  that  must 
be  satisfied  typically  include  constraints  on  initial  and  final  position,  velocity,  and  acceleration, 
i.e.  xi0,x'i0,x,/0,xif,x'if,x'lf  [19].  In  the  case  that  each  of  these  represents  a  given  boundary 
condition  to  be  satisfied,  then  the  minimum  order  N  of  polynomial  P  in  (3.8)  is  5,  since  do  — 
df  —  2.  Thus  all  of  the  coefficients  of  P  will  be  uniquely  defined  by  these  boundary  conditions, 
leaving  r  as  the  only  varied  parameter.  Of  course,  if  needed,  higher  order  derivatives  may  be 
added  to  meet  desired  states  at  initial  and/or  final  points.  This  simply  requires  increasing  the 
polynomial  degree  by  the  number  of  additional  constraint  derivative  orders.  For  example,  fixing 
the  final  jerk  A;"  at  zero  (as  is  the  case  in  many  terminal  trajectory  problems)  increases  df  by  1, 
requiring  a  polynomial  of  order  6. 

Thus  far,  we  have  considered  the  virtual  arc  T  as  the  only  varied  parameter.  This,  however, 
limits  the  flexibility  of  our  reference  functions,  as  it  provides  only  one  optimization  variable  for 
varying  our  reference  trajectories  within  the  set  of  admissible  trajectories.  Greater  flexibility 
may  be  achieved  in  our  reference  trajectory  by  increasing  the  number  of  varied  parameters  to 
be  considered  in  the  optimization  routine.  This  can  be  accomplished  by  "fictitious"  boundary 
conditions  at  the  end  points.  For  example,  to  increase  the  flexibility  in  the  reference  trajectories 


17 


of  the  3D  terminal  trajectory  case  discussed  above,  we  may  add  a  3rd  order  derivative  x'^p  or 
jerk  constraint,  to  the  initial  point.  Thus  do  becomes  3,  and  ./V  =  do  +  df+  1  —  1.  This  additional 
"fictitious"  derivative  can  now  be  used  as  a  varied  parameter,  giving  us  greater  control  over  the 
shape  of  the  candidate  trajectories. 


3.3  The  Virtual  Domain 

We  now  turn  our  attention  to  the  reason  behind  parameterizing  the  reference  functions  with  re¬ 
spect  to  an  arbitrary,  or  virtual  parameter  z.  Up  to  this  point,  we  have  only  concerned  ourselves 
with  the  flexibility  of  the  trajectory  reference  function  itself.  We  must  now  consider  why  pa¬ 
rameterizing  the  reference  functions  with  respect  to  time  t  actually  limits  our  flexibility.  Let  us 
consider  what  would  happen  if  this  were  indeed  the  case  by  analyzing  the  speed  profile  of  the 
resulting  time-parameterized  trajectory.  In  this  case,  the  speed  at  any  time  t  along  the  trajectory 
would  be  given  by 

V(t)  =  \J  u(t)2 +  v(t)2 +  w(t)2  —  \/x2  +  y2+z2  =  \Jp~  +  Py+P?  (3.10) 


Thus  for  any  candidate  spatial  trajectory  given  by  P,  we  have  a  single  unique  and  unalterable 
speed  profile  along  that  trajectory.  This  is  undesirable  since  we  might  want  to  be  able  to  vary 
the  speed  profile  independently  of  the  spatial  trajectory.  By  parameterizing  the  trajectory  with 
respect  to  an  abstract  argument,  in  this  case  the  virtual  arc  parameter  z(z  ^  t),  we  introduce 
a  speed  factor  that  allows  for  just  such  independence  between  speed  and  the  spatial  trajectory. 
This  speed  factor  A  is  given  by 

m  =  %  (3-id 

which  is  essentially  the  virtual  speed.  Now  since 


Xi{z) 


dxi(z) 

dt 


dxj(z)  dz 
dz  dt 


x[(z)Hz), 


(3.12) 


we  have 

V ( t)  =  A  ( z)  \Jx' ( z)2  +  /( t)2  +  z'{z)2  =  A  ( z)  sJP'x2  +  Py2  +  Pz2  (3.13) 

Thus  by  varying  the  speed  factor  A,  we’re  able  to  vary  the  speed  profile  independently  of  the 
spatial  trajectory. 


The  reason  that  the  capability  to  vary  the  speed  profile  is  desirable  warrants  further  discussion. 
In  most  cases,  the  minimum  and  maximum  speeds  of  a  vehicle  are  constrained  by  Vmax  and 
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Vmjn.  In  the  case  of  the  time -dependent  speed  given  by  (3.10),  the  direct  correlation  between 
the  spatial  trajectory  and  speed  along  the  trajectory  means  that  the  speed  constraints  will  also 
constrain  the  spatial  trajectory,  severely  limiting  our  set  of  candidate  trajectories.  Computing 
the  trajectories  in  the  r  domain,  however,  allows  us  to  satisfy  these  constraints  by  adjusting 
A(t)  without  directly  affecting  the  spatial  trajectory,  such  that 

VnUtl  ^  A  (T)  yjp?  +  Pf+P?  ^  Vmax- 

Likewise,  consider  possible  constraints  on  centrifugal  acceleration  acf  =  kV2,  where  k  is  the 
curvature  of  the  trajectory.  Here  again,  solving  in  the  time  domain  ties  the  spatial  trajectory 
directly  to  speed  V,  and  thus  acf.  Moving  to  the  r  domain,  however,  allows  us  to  again  satisfy 
this  constraint  independently  of  the  spatial  trajectory  by  adjusting  A(t),  since 

acf( t)  =  £(t)V2(t)  =  £(t)A2(t ){P'X2+Py2+P'z2)  <  acJiax. 


Yakimenko  [20]  makes  note  of  some  important  points  concerning  this  virtual  parameterization. 
First,  if  the  virtual  arc  length  r f  is  on  the  order  of  physical  path  length  sj  .  then  the  virtual  speed 
A(t)  will  be  correspondingly  congruent  with  the  physical  speed  along  the  trajectory.  Thus  if 
we  set  Ao  equal  to  Vq,  we  can  expect  xy  ~  Sf.  This  conclusion  enables  us  to  make  a  sound 
initial  guess  on  r y  during  the  optimization  portion  of  the  routine.  Second,  we  can  see  from  the 
relationship  between  T  and  time  given  by  equation  (3.11)  that  time  can  be  expressed  as 


t 


d  T 


Jo  A(t)' 


Thus  varying  the  speed  factor  A  (  t)  not  only  changes  the  speed  profile,  but  the  time  scale  as  well. 
Finally,  we  can  use  the  relationship  between  the  virtual  domain  and  time  domain  in  equation 
(3.1 1)  to  determine  the  derivatives  in  the  time  domain  of  any  time- variant  parameter  [21].  Given 
parameter  £,  the  first  and  second  time-derivatives  of  this  parameter  are 

f  M  =  ^  =  C'MMt)  and  f(r)=A(V  i'  +  H")  (3.14) 


We  may  then  invert  equation  (3.14)  to  transfer  the  boundary  conditions  from  the  time  domain 
into  the  r  domain: 


Cj  =  y  and  C," 
A 


A2  A 


(3.15) 
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3.4  Inverse  Dynamics 

Once  the  set  of  candidate  trajectories  has  been  computed,  the  next  step  in  the  DM-IDVD  routine 
is  to  compute  the  components  of  the  state  vector  and  the  controls  vector  at  discrete  points  along 
each  candidate  trajectory.  In  so  doing,  not  only  do  we  ensure  that  the  constraints  of  (3.5)— (3.7) 
are  not  violated,  but  we  also  explicitly  return  the  controls  required  to  follow  each  candidate 
trajectory.  What  we  are  essentially  doing  is  computing  the  time-histories  of  the  states  and 
controls  along  the  trajectory.  This  is  accomplished  by  way  of  inverting  the  dynamic  equations 
of  motion  for  the  vehicle.  Let  us  again  consider  the  case  of  a  simple  6  Degree  of  Freedom 
(6DOF)  vehicle  operating  in  3D  cartesian  space.  Suppose  the  kinematics  of  the  vehicle  are 
given  by 

x(t)  u(t) 

y(t)  =ubR  v(r)  (3.16) 

.  tit)  J  L  w(o . 

where  l!R  is  the  rotation  matrix  from  the  body  frame  {/?  }  to  the  NED  local  tangent  plane  frame 
{«},  and  is  defined  by  two  Euler  angles,  pitch  (0(t))  and  yaw  (t jf(t))  (for  simplicity,  we  will 
neglect  roll  angle  (0(f)): 

cos  \j/(t)  cos  9{t)  — sint \jf{t)  cos  \jf(t)  sin  6(t) 
uhR  =  sin  t/f(f)  cos  0(f)  cos  tyf(f)  sin  t/f(f)  sin  0(f)  (3.17) 

—  sin  0(f)  0  cos  0(f) 

Before  applying  inverse  dynamics,  we  must  transfer  (3.16)  to  the  r  domain  (assuming  a  constant 
surge  velocity  Uq): 

'  x'(t)  1  I-  Uq 

A(t)  y'{  t)  =ubR  v(T)  (3.18) 

_  z\ t)  _  _  w{ t) 

If  we  also  assume  that  pitch  angle  is  small  enough  such  that  sin  0(f)  ~  0  and  cos  0(f)  «  0,  (3.17) 
becomes 

cost/r(r)  —  sint/r(T)  0 

bR(  t)—  sinyr(T)  cosv/(t)  0  (3.19) 

0  0  1  _ 

Note  that  as  all  kinematic  parameters  and  equations  have  now  been  converted  to  the  T  domain, 
for  simplicity  the  notation  for  functional  r-dependency  will  be  hereafter  omitted.  Inverting  the 
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kinematic  equations  (3.16)  produces  the  following  inverse  dynamics  equation: 


'  U0  ' 

cos  t/A 

sint/A 

0  ' 

~  x?  ' 

V 

=  X 

—  sint/A 

cos  Y 

0 

i 

(3.20) 

w 

0 

0 

1  _ 

_  z!  _ 

The  remaining  inverse  dynamics  equations  are  arrived  at  by  solving  (3.20)  for  the  three  un¬ 
known  parameters,  v,  w,  and  t/A.  By  inspection,  we  can  readily  ascertain  that 

w  =  Xz',  (3.21) 

whereas  the  solutions  for  the  other  two  unknown  parameters  are  not  as  easily  achieved.  By  way 
of  a  geometric  analysis  of  the  scalar  product  of  the  vectors  on  the  right  hand  side  of  (3.20),  we 
arrive  at  the  following  solutions  for  v  and  y/: 

v  =  ^V-(x'2  +  y'2)~U2  (3.22) 

and 

i  v/A  i  v 

Y  =  T*  — tan  1  jj—jir  =  VP  — tan-1  —  (3.23) 

(Vo/  A  U o 

where  / 

vP  =  tan^1^7.  (3.24) 

x 

With  the  kinematic  equations  thusly  inverted,  we  are  now  able  to  verify  that  the  constraints  on 
these  states  are  not  violated  by  the  candidate  trajectories. 


3.5  Reference  Function  Coefficients 


States  at  the  boundary  conditions  are  now  evaluated  so  that  we  may  determine  the  coefficients 
of  the  reference  functions  (3.8).  Initial  and  final  conditions  on  the  coordinates  (x,o  and  x,f)  are 
given  by  equations  (3.1)  and  (3.3).  Known  or  given  boundary  conditions  on  velocity  compo¬ 
nents  of  surge,  sway,  and  heave  define  the  first-order  r-derivatives  of  the  coordinate  states  (x'j{), 
x \f )  by  way  of  (3.18): 


x'o ;/ 

U  D 

_  bRo-,f 

'  U0  ' 

y  0;/ 

At);/ 

v0 ;/ 

L  Z  0;/  J 

.  w0 ;/  . 

(3.25) 
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where  the  initial  and  final  values  of  yaw  required  to  determine  1uRq-j  above  are  given  by  (3.23): 


Vo ;/ 


T'o  —  tan 


1  VQ:/ 

Uo 


and  those  for  pitch  are  found  by: 


(3.26) 


_i  — Wn  -f 

do  -f  —  Yo  +  tan  1  — - - — .  (3 .27) 

\Juo  +  vo ;/ 

The  second  order  derivatives  at  the  initial  point  (jc'q)  are  typically  provided  by  vehicle  motion 
sensors  (accelerometers),  although  they  may  be  explicitly  stated  as  a  given  initial  condition, 
while  the  final  second  order  derivatives  are  usually  given  as  defined  desired  final  con¬ 
ditions  on  acceleration.  Likewise,  the  initial  and  final  higher  order  derivatives  required  by  the 
chosen  reference  functions  are  are  either  assigned  "guessed"  values  if  they  represent  the  variable 
parameters  (OP’s),  or  are  explicitly  given  as  dictated  by  the  constraints  on  trajectory  behavior 
or  constraints  on  the  controls  at  the  initial  and  terminal  points.  All  of  the  derivatives  must  be 
converted  into  the  z  domain  as  required  by  the  method  describe  above.  With  all  necessary 
states  and  their  T  derivatives  have  been  defined  for  the  initial  and  terminal  points,  along  with 
guesses  for  the  variable  parameters  (including  T y),  the  coefficients  are  then  found  by  solving 
the  appropriate  system  of  equations  defined  by  (3.8)  for  these  coefficients. 


3.6  Discretization 


In  order  to  numerically  calculate  the  remaining  states  over  the  length  of  the  virtual  arc  from 
T  =  0  to  z  =  Tf,  it  is  necessary  to  discretize  the  trajectory  into  A  evenly  spaced  points  (in  the  r 
domain)  with  intervals  of 


At 


V 

n- r 


(3.28) 


such  that 

Zj  =  Ty  —  |  +  At,  j  =  2,...,N,  (ti  =0).  (3.29) 


The  At  for  each  interval  may  now  be  determined: 


At  ;__i  =  sqrt 


(Xj 


-i)2  +  (yj-yj-i)2  +  (.zj-zj-i) 


A?  +  vi 


+  W^_ 


(3.30) 
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The  speed  factor  A  for  each  interval  by  way  of  the  discrete  version  of  (3.1 1): 


At 

Atj-i 


(3.31) 


The  value  of  A  in  (3.25)  may  be  assumed  to  be  any  reasonable  value,  such  as  Xq-j  =  1,  since 
this  value  merely  scales  the  virtual  domain.  In  other  words,  the  higher  the  value  assigned  to  A, 
the  larger  the  resulting  value  of  T y.  This  result  is  due  to  the  time-virtual  domain  relationships 
given  by  (3.11)  and  (3.13).  Thus  we  can  see  that  ^  where  Sf  is  the  total  physical  path 
length  along  the  trajectory.  [10]  Once  the  trajectory  has  been  discretized  and  A j  for  each  point 
computed  by  (3.31),  we  may  compute  the  states  and  controls  at  all  intermediate  point  at  each 
time  stamp  along  the  trajectory  by  way  of  the  inverse  dynamic  equations  determined  previously 
((3.21)  through  (3.23)  in  the  6DOF  3D  vehicle  case  above).  In  some  cases,  numeric  analysis 
may  be  required  to  calculate  the  time  derivatives  of  certain  parameters  in  order  to  perform 
constraint  checks  on  these  parameters.  Such  is  the  case  with  'P  in  the  3D  example  case  above, 
where  we  must  numerically  differentiate  (3.24)  in  order  to  check  a  given  constraint  on  'P  such 
as  I'S'MI  <  'i* max-  The  net  result  is  a  time-history  of  the  states  and  controls  at  uniform  discrete 
points  along  the  trajectory. 


3.7  Optimization 

Obviously,  the  whole  of  the  entire  above  process  generates  the  necessary  set  of  trajectories  sub¬ 
ject  to  (3.1)  to  (3.4),  we  must  now  turn  our  attention  to  satisfying  the  constraints  given  by  (3.5) 
through  (3.7),  along  with  any  optimization  parameters  required  by  the  situation.  As  the  goal  of 
Direct  Methods  is  to  determine  a  single  near-optimal  trajectory  subject  to  these  constraints  and 
OPs,  we  must  invoke  some  sort  of  optimization  routine  that  will  optimize  a  defined  performance 
index  and  penalty  (cost)  function.  This  routine  will  accept  the  set  of  trajectories  (more  precisely, 
the  state  and  control  time  histories),  constraints,  and  OPs  as  input,  and  output  the  trajectory  that 
most  closely  (near-optimally)  satisfies  the  given  requirements. 

Let  us  first  consider  the  cost  function.  We  desire  that  our  solution  trajectory  meet  certain  re¬ 
quirements  or  not  violate  certain  constraints.  The  cost  function  is  how  we  quantify  such  con¬ 
straint  violations  or  satisfaction  of  requirements.  The  cost  function  determines  how  much  a 
given  constraint  or  requirement  is  violated,  and  calculates  an  associated  penalty  value.  Let’s 
consider,  for  example,  an  example  of  an  Unmanned  Underwater  Vehicle  (UUV),  defined  by  the 
equations  discussed  previously  in  the  6DOF  3D  case.  Suppose  we  place  the  obvious  constraints 
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on  vehicle  depth  z  (measured  in  the  NED  coordinate  frame) 


zmin  <  Zj  <  0, 


(3.32) 


pitch  0 

and  yaw  rate  y/ 


0(01  ^  Qmax 


\V(t)\  <  fymax- 

Penalties  for  each  may  be  expressed  as 


(3.33) 

(3.34) 


min  (0 ,Zj  Zmin)- 
J 

min  (0;  - Zj )2 
j 

max  (0;  |07j  -  6/  max) 2  and 
j 

max  (0;  y/j  y/j  max)  ■ 

j 


(3.35) 


The  resulting  cost  function  multiplies  these  by  a  related  scaling  (weighting)  parameter  k: 


A 


kz  kz  k6  kV 

A  .  A  ,  A.  ,  A 


min  (0 ,Zj  Zmin)- 

j 


min(0  -,~Zj)2 
j 

max  (0;  1 0/1  0/ max , 

j 

max (0-\y/j  \  -yfjm.dX 


2 


(3.36) 


Other  parameters  that  may  be  penalized  subject  to  minimums  and/or  maximums  might  be  speed 
components  (umin  <  uj  <  umax,  vmin  <  vj  <  vmax,  etc.),  bank  angle  (\0,\  <  (j)max),  or  the  controls 
m  ^  0/ tui.x ) ^  <rs  well  as  their  denvatrves  (actuator  dynamics)  ( | dj  ^ max )• 


The  performance  index  factors  in  those  parameters  that  are  to  be  minimized  or  maximized  based 
on  the  specific  requirements  of  the  problem,  such  as  final  time  tf,  or  some  other  parameter  spe¬ 
cific  to  the  application,  such  as  in  the  case  with  this  thesis,  Information  Gain.  In  this  case,  since 
we  will  attempt  to  maximize  the  IG,  the  performance  index  for  IG  will  simply  be  a  weighting 
factor  w'8  times  the  actual  calculated  value  for  IG. 
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Once  the  overall  cost  function  and  performance  index  is  formulated,  optimization  proceeds  by 
utilizing  some  optimization  algorithm,  such  as  those  mentioned  at  the  beginning  of  this  chapter. 
An  example  of  this,  and  the  method  chosen  for  use  in  this  thesis,  is  the  fminsearch  algorithm  in 
MATLAB.  Optimization  is  achieved  by  inputing  initial  guesses  for  the  variable  (optimization) 
parameters,  choosing  the  required  number  of  iterations,  and  running  th e,  fminsearch  algorithm 
on  the  function  that  contains  the  trajectory  generation  code,  the  inverse  dynamics  calculations, 
and  the  combined  cost  function/performance  index  (hereafter  referred  to  simply  as  PI).  The  rou¬ 
tine  then  seeks  to  minimize  the  PI  by  varying  values  of  the  "guessed"  optimization  parameters. 
The  end  result  is  a  solution  containing  the  final  guesses  for  the  OP’s  and  the  time-histories  of 
the  states/controls  corresponding  to  the  near-optimal  (minimized)  PI. 
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CHAPTER  4: 

Algorithm  Implementation  and  Testing 


This  chapter  describes  the  path  planning  algorithm  for  the  REMUS  AUV  developed  for  this  the¬ 
sis.  The  algorithm  implements  the  previously  described  concepts  of  Direct  Methods,  Bayesian 
inference,  probabilistic  analysis,  occupancy  grids,  and  information  gain.  The  algorithm  takes 
a  known  "world  map"  of  the  environment  as  input,  as  well  as  user  specified  constraints  and 
boundary  conditions  on  the  vehicle’s  kinematic  and  spatial  parameters.  The  net  result  is  a  tra¬ 
jectory  that  satisfies  the  specified  constraints  on  the  vehicle  and  optimizes  the  information  gain 
achieved  by  the  vehicle’s  sensors.  Figure  4.1  shows  a  simplified  block  diagram  of  the  routine. 
The  orange  box  is  the  DM-IDVD  path  planning  and  optimization  routine.  The  white  box  within 
this  box  constitutes  the  optimization  loop  that  is  run  within  the  optimization  function.  The  gray 
bubble  corresponding  to  the  gray  box  in  the  optimization  loop  displays  the  probabilistic  analysis 
subroutine,  which  performs  the  sonar  imaging,  occupancy  grid  updating,  information  gain  cal¬ 
culation,  heuristics  violation  analysis,  and  obstacle  avoidance  subroutine.  Each  component  will 
be  further  described  in  this  chapter.  The  entire  routine  is  run  using  a  set  of  MATLAB  scripts  and 
functions  developed  by  the  author  specifically  for  this  thesis,  as  well  as  a  couple  of  off-the-shelf 
scripts  sourced  from  the  Mathworks  website  which  offer  simple  functionality  for  specific  tasks 
(such  as  generating  a  Bresenham  line  in  the  sonar  sweep  and  obstacle  avoidance  subroutines). 
All  of  the  pertinent  MATLAB  code  used  in  this  thesis  is  presented  in  Appendix  A. 

4.1  Problem  Setup 

The  simulation  environment  chosen  for  this  thesis  was  the  testing  tank  located  in  the  basement 
of  Halligan  Hall  on  the  campus  of  the  Naval  Postgraduate  School.  The  tank  measures  approx¬ 
imately  10  meters  by  20  meters  by  2  meters  deep.  The  tank  is  modeled  in  MATLAB  in  matrix 
form  for  the  purposes  of  generating  simulated  sonar  images  and  the  occupancy  grid.  All  posi¬ 
tional  coordinates  of  the  vehicle  are  analyzed  with  respect  to  the  center  of  gravity  of  the  vehicle, 
which,  for  the  sake  of  simplicity,  corresponds  to  the  vertical  (z-axis)  center  of  rotation. 

As  will  be  discussed  in  Section  4.2.1,  the  simulation  was  run  with  the  vehicle  starting  with  a 
known  and  fixed  set  of  initial  conditions  on  position  and  heading.  While  the  choice  for  the 
initial  state  is  completely  arbitrary,  that  for  the  final  condition  is  not.  In  theory,  the  number  of 
candidate  endpoints  is  quasi-infinite,  bounded  only  by  the  constraints  of  the  test  space  which, 
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Figure  4.1:  Overall  Trajectory  Generation  and  Optimization  Routine 

in  the  case  of  this  thesis,  is  the  test  tank.  Although  the  simulation  constrains  the  trajectories  of 
the  vehicle  to  the  confines  of  the  test  tank,  a  means  must  be  addressed  by  which  an  endpoint  of 
the  set  of  candidate  trajectories  evaluated  by  the  direct  methods  routine  may  be  chosen  from  the 
quasi-infinite  set  of  endpoints.  Thus  another  constraint,  a  time  horizon  (TH),  is  incorporated. 
The  TH  is  a  notional  period  of  time  over  which  we  will  limit  the  vehicles  motion  and  perform 
the  required  analysis.  All  trajectories  of  the  candidate  trajectory  sets  will  be  confined  to  the  set 
of  endpoints  that  the  vehicle  can  possibly  reach  given  constraints  on  it’s  surge  (u)  and  sway  (v) 
velocities.  Since  umax  is  greater  than  vmax  for  the  REMUS  vehicle,  the  set  of  possible  endpoints 
is  confined  to  lie  within  an  arc  represented  by  the  TH  multiplied  by  the  umax.  A  graphical 
representation  of  the  random  endpoints  can  be  seen  in  Figure  4.2. 

Now,  while  it  is  possible  to  implement  the  DM-IDVD  trajectory  optimization  routine  with  free 
endpoint  coordinates  given  as  variable  parameters,  the  computational  requirements  for  such  a 
routine  would  be  exponentially  increased,  as  this  also  leaves  the  first  order  time  derivatives  of 
the  coordinates  at  the  endpoints  free  to  vary  as  well.  Thus  this  approach  is  not  feasible  for  this 
application,  especially  given  the  computational  burden  already  placed  on  the  resources  by  the 
subordinate  components  of  the  routine  added  to  the  standard  DM-IDVD  algorithm  by  this  thesis 
(i.e.  the  sonar  imaging,  probabilistic  analysis,  and  information  theoretics).  Thus,  when  setting 
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Figure  4.2:  Randomly  Generated  Endpoints  (red  circles  are  the  endpoints,  black  star  is  the  start  point; 
pink  circle  is  the  time  horizon  arc,  blue  rectangle  is  the  test  tank) 

up  the  problem,  it  became  necessary  to  further  constrain  the  endpoint  coordinates  to  a  smaller 
region.  In  order  to  accomplish  this,  an  initial  analysis  of  the  information  space  was  performed 
to  determine  where  the  endpoints  are  that  achieve  the  greatest  information  gain  for  simple  tra¬ 
jectories.  This  was  accomplished  by  generating  a  set  of  random  endpoints  that  lie  within  the 
confines  defined  by  the  tank  walls  and  the  TH  arc.  A  simplified  iterative  routine  was  run  that 
traverses  the  vehicle  from  the  fixed  starting  point  to  each  of  the  random  end  points.  Each  end¬ 
point  trajectory  was  also  iterated  over  a  fixed  number  for  randomly  generated  final  yaw  angles 
(y //),  in  order  to  maximize  our  knowledge  of  the  information  space  over  all  variable  parameters. 
A  map  of  the  information  space  for  each  value  of  t f/f  was  generated  by  evaluating  the  informa¬ 
tion  gain  achieved  along  that  specific  trajectory.  A  simplified  algorithmic  representation  of  this 
method  is  shown  in  Algorithm  1 .  The  resulting  map  is  a  3D  representation  of  IG  as  a  function 
of  the  endpoint  coordinates  for  each  value  of  t f/f.  Two  dimensionally,  an  example  of  this  can 
be  seen  in  Figure  4.3.  This  figure  is  a  "heat  map"  where  each  cell  of  the  figure  represents 
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Algorithm  1  Information  Space  Initial  Analysis  Algorithm 

1.  For  i  =  1  to  number  for  random  y/y’s 

2.  Generate  empty  IG  Array 

3.  For  k  —  1  to  number  of  random  endpoints 

4.  Generate  straight-line  trajectory  between  start  and  endpoint(k) 

5.  At  each  discrete  point  along  trajectory: 

6.  Perform  sonar  sweep  subroutine  to  generate  sonar  image 

7.  Run  OG  Update  subroutine  on  sonar  image 
— »  Update  OG 

— >  Calculate  IG  at  this  point  and  add  it  to  previous  IG  value 

8.  Store  cumulative  IG(k)  value  in  cell  corresponding  to  endpoint(k) 

9.  end  loop 

10.  end  loop 


a  cartesian  coordinate  and  the  value  information  gain  of  each  coordinate  is  represented  by  a 
color,  with  cooler  colors  representing  lower  values  and  hotter  colors  representing  higher  values. 
A  3D  version  of  this  information  gain  mapping  can  be  seen  in  Figure  4.4.  By  inspection  of  this 
IG  mapping  for  each  of  the  values  of  y/y,  it  became  clear  that  maximal  information  gain  was 
achieved  with  trajectories  terminating  in  the  Southwest  corner  of  the  TH-constrained  operating 
space.  Thus  the  values  for  the  final  coordinates  are  placed  in  this  general  location  throughout  the 
rest  of  the  algorithm  testing.  In  a  real-world  application  where  this  a  priori  data  is  not  achiev¬ 
able,  other  methods,  such  as  heuristic  evaluations,  known-space-constrained  explorations,  etc., 
may  be  used  to  determine  the  desired  end  point. 


4.2  Direct  Methods  Problem  Formulation 

4.2.1  Definition  of  States,  Constraints,  and  Boundary  Conditions 

As  discussed  in  Section  3.1,  the  first  step  was  to  define  the  vehicle  state  vector,  controls,  and 
equations  of  motion  in  state  space  form.  As  this  thesis  analyzes  only  two-dimensional  trajecto¬ 
ries  in  the  horizontal  plane,  the  state  vector  x  was  defined  as 

x(t)  =  [x{t),y(t),yf{t),u{t),v{t)]T ,  (4.1) 


and  the  corresponding  ordinary  differential  equations  of  motion  are 


x(t) 

y(t) 


u[t) 

v(t) 


(4.2) 
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Figure  4.3:  Information  Gain  Heat  Map  for  i jff  =  —45°  (NED)  Red  colors  indicate  higher  IG  values, 
blue  colors  indicate  lower  IG  values 


where  uhR  is  given  by 


tm 


cos  —sin  y/(t) 
sin  \j/(t)  cos 


The  controls  vector  is  given  by 


U  [ft  prop-  ft  fit  i  ftslt\ 


(4.3) 


(4.4) 


where  nprop,  nm,  and  ns[t  are  the  controls  (thrusts)  on  the  main  propeller,  forward  lateral  cross¬ 
body  thruster,  and  stern  lateral  cross-body  thruster,  respectively.  Next,  the  boundary  conditions 
at  the  initial  and  final  points  were  defined.  The  initial  pose  of  the  vehicle  was  arbitrarily  defined 
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Figure  4.4:  Three  Dimensional  Map  of  Information  Gain  for  y/y  =  —45°  (NED)  Red  colors  indicate 
higher  IG  values,  blue  colors  indicate  lower  IG  values 

with  a  fixed  starting  point  and  heading: 

x(t  =  0)  =  X0  =  Xinit 
y(t  =0)=yo  =  yinit 
V(t  =  0)  =  Yo  =  psiwt 
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where  x;mf,  and  psiinit  are  the  values  for  these  parameters  specified  in  the  main  MATLAB 
script  (see  Appendix  A).  Likewise,  the  final  pose  is  defined  by: 


x(t  tf)  —  Xf  X final 

>’(f  =  f/0)  =  >’/  =  y final 
V(t  =  tf)  =  Yf  =  PSl  final 


While  the  value  used  for  psifinai  was  initially  chosen  somewhat  arbitrarily  in  the  interest  of 
simplifying  the  problem  (and  later  implemented  as  a  variable  parameter),  as  discussed  in  Sec¬ 
tion  4.1,  the  choice  of  values  for  Xfmai  and  is  not  arbitrary.  These  values  were  chosen 
based  on  the  initial  information  space  analysis  described  in  that  section. 

Kinematic  constraints  of  the  vehicle  were  sourced  from  Doherty  [22]: 

Table  4.1:  Kinematic  Constraints 


Constraint 

Value 

Max  Surge  Velocity  (vmax) 

2.88  m/s 

Max  Sway  Velocity  (umax) 

0.5  m/s 

Max  Yaw  Rate  (\jfmax) 

14.5  deg/sec 

4.2.2  Reference  Functions 

The  first  step  in  the  DM-IDVD  process,  represented  by  the  first  block  in  Figure  4. 1,  is  to  choose 
the  reference  functions  for  the  key  spatial  parameters  of  the  trajectory.  The  key  parameters 
of  concern  in  this  case  were  Euclidean  spatial  coordinates  x  and  y,  and  yaw  angle  if/.  Thus 
reference  functions  for  these  parameters  were  developed.  Additionally,  a  reference  function 
for  the  speed  factor  A  was  also  chosen  to  allow  for  maximum  flexibility  in  the  speed  profile 
along  the  trajectory.  As  discussed  in  Section  3.3,  the  reference  functions  are  parameterized 
in  the  virtual  domain  as  a  function  of  the  virtual  parameter  T.  The  reference  function  for 
the  coordinates  x-,  (i  =  {1,2},  sox  =  x\  and y  =  X2 )  are  shown  in  equation  (4.5)  below.  The 
chosen  reference  function  for  the  spatial  coordinates  is  a  third  order  polynomial  combined  with 
two  trigonometric  sine  functions.  The  addition  of  the  sine  functions  increases  the  flexibility  in 
varying  the  curvature  of  the  resulting  trajectory.  This  reference  function  is  then  differentiated 
twice  to  allow  the  final  acceleration  in  each  coordinate  to  be  varied.  (The  resulting  cosine 
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terms  in  the  second  equation  of  (4.5)  arise  from  the  first  derivative  of  the  sine  terms  in  the  first 
equation.) 


Xj( t)  :  Pi(f)  =al0  +  a\r  +  al2T2  +  al3;t3 +  b\sm(nr)  +  bl2sm(2nr) 

t)  :  XfP{(x)  =  a\+2al2T-\-3al3i2 +  nb\cos(nt)  +  2nb2Cos(2nz)  (4.5) 
x"i( t)  :  t2P/'(t)  =  2<72  +  6r/3f-^2Z?'1sin(^T)  +  4^:2^2sin(2^:T) 


where 


T  = 


T 


The  parameter  f  is  used  as  opposed  to  r  in  order  to  simplify  the  process  of  solving  for  the 
coefficients  at  To  and  r y  in  the  sine  and  cosine  components  of  the  reference  function.  The 
same  parameter  was  used  for  all  of  the  reference  functions  for  consistency,  despite  the  lack  of 
trigonometric  terms  in  the  y/  and  A  reference  functions.  The  next  steps  given  by  equations  (4.6) 
and  (4.7)  substitute  To  and  T y  for  T  into  (4.5)  to  develop  the  matrix  equation  (4.8).  (Note  that 
although  equations  (4.6)  through  (4.8)  are  those  for  the  x  coordinate,  those  for  y  are  of  the  same 
form.) 


o 

II 

(-J 

KT 

H 

■TO  =  «0 

© 

II 

Tfx'0  =  a\  +  nb\  +  2nb\ 

(4.6) 

o 

II 

A7 

tfxo  —  2a\ 

(i'¬ 

ll 

i(7 

xy  —  a0  +a\  +  ci 9  T  ci3 

(i'¬ 

ll 

(^ 

i(7 

T yxf  =  a]  4-  2a'2  +  3^  -  7T/?j  +  2^/?') 

(4.7) 

i'¬ 

ll 

(^ 

A7 

TyXy  =  2«2  +  6a‘3 
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The  above  equations  are  then  put  into  matrix  form  as  seen  in  (4.8): 


"  1 

0 

0 

0 

0 

0 

< 

Xq 

0 

1 

0 

0 

n 

2k 

a\ 

Ay 

0 

0 

2 

0 

0 

0 

a2 

x0  Lf 

1 

1 

1 

1 

0 

0 

a3 

y 

0 

1 

2 

3 

-7T 

2k 

b\ 

x'fTf 

0 

0 

2 

6 

0 

0 

hx 

L  °2  \ 

1 

t4 

(4.8) 


The  following  boundary  conditions,  as  well  as  the  initial  guesses  for  the  varied  parameters 
x'j-  and  y'J-  are  then  substituted  into  (4.8)  and  the  reference  function  coefficients  are  solved  for 
(second  step  of  DM-IDVD  illustrated  by  the  second  block  of  Figure  4.1). 


Xq  —  Xinit 

Xf  Xfinai 

x'0  =  0 

X'f  =  0 

(4.9) 

A  =  0 

x'j  =  var 

JO  —  yinit 

y f  —  y  final 

A  =  0 

y'f  = 0 

(4.10) 

>’o  =  0 

y'j  —  var 

where  Xfinai,  y^n ,  and  yjinai  are  the  chosen  trajectory  start  point  and  end  point  values 
discussed  in  Section  4.2.1.  The  reference  function  for  t/r  was  chosen  to  be  a  simple  fifth  order 
polynomial.  Since  it  was  desired  to  vary  the  initial  and  final  second  derivatives  of  t/r,  a  fifth  order 
polynomial  was  necessary  due  to  the  requirement  given  by  (3.9).  The  development  process  of 
the  reference  function  including  solving  for  the  coefficients  is  identical  to  that  described  above. 

Reference  Function  for  iff: 

yr(f)  =  Pyr( t)  —  ciq  +aj r  +  a f  t2  +  t3  +  aj T4  +  a ^  T5 

)/(t)  =  TfPy'{T)  =  aj  +  2afi  +  3ajt: 2  +  4a  Jt 3  +  5ajf4  (4.11) 

=  TfPv"(f)  =  2  cij  +  6  aj  T  +  12  aj  T2  +  20  T3 
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As  before,  To  and  T y  are  substituted  into  (4.11)  to  get  the  system  of  equations  corresponding  to 
the  boundaries: 


V(*)\t=0 

y/ 

:  Vo  =  a(j 

II 

o 

:  zf  Vo  =  aj 

(4.12) 

II 

o 

:  TjVo  =  2 

V(t)U=Tf 

y/ 

Vf  —  a0 

+  aj  +  a^  +  a^  +  aj  +  aj 

</(?)!  T=Tf 

ZfV'f  =  a'\  +  2a^  +  3aJ  +  4  aj  +  5aJ 

(4.13) 

V"W\  x=tf 


T?  =  2aY  +  6  aY  +  !2aY  +  20  a\ 


1  0  0  0  0  0 

0  1  0  0  0  0 

0  0  2  0  0  0 

11111  1 
0  1  2  3  4  5 

0  0  2  6  12  20 


Vo  V 
Y"qt} 


y'fTf 

V"fzj 


(4.14) 


The  boundary  conditions  on  v  and  the  first  and  second  derivatives  are: 


Vo  =  psiinit 

Vo  =  0 
Vo  =  var 


Vf  =  Psi final 
Vf  =  var 


(4.15) 


where,  as  before,  the  values  for  psiuul  and  psifinai  were  developed  in  Section  4.2.1,  and  the 
initial  and  final  second  derivatives  are  left  as  varied  parameters.  As  with  x  and  y,  these  bound¬ 
ary  conditions  along  with  the  initial  guesses  on  Vq  and  Vf  are  substituted  into  (4.14)  and  the 
coefficients  solved  for. 

The  reference  function  for  the  speed  factor  A  is  of  the  exact  same  form  as  that  for  y/.  and  is 
identically  developed,  and  thus  will  not  be  described  in  detail  here.  The  boundary  conditions 
on  A  are  also  of  the  same  form,  with  Aq  =  /amiW-;  and  Ay  =  lamjjnai  being  set  equal  to  one. 
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This  allows  for  a  direct  correspondence  between  the  speed  factor  and  initial  and  final  speeds,  as 
discussed  in  Section  3.3.  The  initial  and  final  first  derivatives  of  A  are  set  to  zero,  and  the  initial 
and  final  second  derivatives  are  variable  parameters. 


4.3  Inverse  Dynamics 

Once  the  reference  functions  are  fully  developed  as  functions  of  T  as  discussed  in  the  preceding 
section,  they  are  evaluated  over  r  =  [0;  Zf\.  The  result  is  a  fully  defined  and  discretized  tra¬ 
jectory  in  x,  y,  and  t/r,  with  the  necessary  T-domain  derivatives  being  evaluated  concomitantly. 
The  next  step,  illustrated  by  the  third  block  of  Figure  4.1,  is  to  compute  the  remaining  states 
and  controls  via  inverse  dynamics.  Values  for  heading  (VF),  path  angle  (j8),  surge  velocity  (u), 
sway  velocity  (v),  total  speed  (V),  and  time  (t),  as  well  as  the  time  derivatives  of  the  angles  are 
calculated  at  each  discrete  point  k  along  the  trajectory  by  the  following  set  of  inverse  dynamic 
equations: 


t(1)=0 

z{k  >  1)  =z(k—  1)  +At 
2Az 

dt~X{k-  1)  +  A(*) 

f(l)=0 

t(k  >  1)  =t(k—  1)  +dt  (4.16) 

m  =f  (*)  -  Y(k) 

V(k)=X(k)  \/W))2+(/(*))2 
u(k)  —V(k)  sin(j8 (k)) 
v(k)  —V(k)cos(f5(k)) 
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Figure  4.5:  Trajectory  Angles  in  the  Horizontal  Plane 

and  the  time  derivatives  of  the  angles  are  calculated  by 

m/r;  N  _*'(%"(£) -y\k)x"(k) 

[  }  {y'{k)Y  +  {x'{k)Y 

'f'(fc)  =X{k)m'(k) 

\ j/(k)  =X(k)\]/'(k) 

$  =^-1 if 


(4.17) 


The  angular  relationships  are  arrived  at  by  the  motion  dynamics  of  the  vehicle  in  the  horizontal 
plane.  Consider  Figure  4.5.  In  this  figure,  V  is  the  velocity  vector  of  the  vehicle  in  the  NED 
local  tangent  plane  (LTP),  and  u  and  v  are  the  surge  and  sway  velocities  respectively.  The  yaw 
angle  y  is  defined  as  the  direction  that  the  positive  x-axis  of  the  vehicle  in  the  body  centered 
frame  {b }  makes  with  the  Northerly  axis  of  the  LTP.  The  heading  angle  is  the  angle  between 
the  North  axis  of  the  LTP  and  the  velocity  vector  V.  /3  is  defined  as  the  angle  between  the  the 
velocity  vector  and  the  positive  x-axis  of  the  vehicle  in  { b } .  Thus  /)  =  VF  —  y. 

4.4  Path  Probabilistic  Analysis 

The  probabilistic  analysis  portion  of  the  routine  occurs  immediately  after  the  inverse  dynamics 
calculations.  It  consists  of  several  subroutines  that  are  executed  at  each  discrete  point  along  each 
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trajectory.  This  part  takes  as  inputs  the  vector  of  coordinate  values  (x.y)  of  the  geometric  center 
of  the  vehicle  along  the  entire  trajectory,  and  the  corresponding  vector  of  yaw  angles  y/  for  each 
point.  It  executes  subroutine  functions  at  each  point  for  performing  a  sonar  sweep,  updating  the 
occupancy  grid,  and  calculating  information  gain,  as  well  as  performing  the  necessary  analyses 
for  heuristics  violations  and  obstacle  avoidance. 

4.4.1  Generating  the  Sonar  Image 

In  order  to  generate  a  simulated  sonar  image  of  the  environment,  a  "World  Model"  was  first 
constructed  to  represent  the  spatial  environment  that  the  vehicle  was  to  be  operated  in.  For  this 
thesis,  that  environment  was  to  be  a  model  of  the  Center  for  Autonomous  Vehicles  Research 
(CAVR)  water  test  tank  located  in  the  basement  of  Halligan  Hall  on  the  NPS  campus.  This  open 
top  tank  measures  approximately  10  meters  by  20  meters.  The  tank  is  modeled  in  MATLAB 
with  a  simple  matrix.  The  matrix  cell  values  at  and  outside  the  tank  walls  have  a  value  of  1, 
while  those  inside  the  walls  (representing  water  space)  have  a  value  of  0.  The  sonar  image 
is  generated  by  a  MATLAB  function  that  performs  a  sonar  sweep  against  the  World  Model 
( sweep. m ).  The  sonar  sweep  function  casts  a  single  sonar  ray  along  a  Bresenham  line  that  has 
a  length  equal  to  the  effective  range  of  the  sonar.  If  all  World  Model  cells  along  this  ray  hold 
a  value  of  zero,  then  the  function  chooses  a  random  value  from  an  equally  distributed  range  of 
possible  signal  strength  values  for  a  “no-object-in-beam"  condition.  Conversely,  if  a  cell  with 
a  value  of  1  is  found  along  the  ray,  then  the  function  chooses  a  random  value  from  an  equally 
distributed  range  of  possible  signal  strength  values  for  a  “no-object-in-beam"  condition.  The 
ranges  for  “object-in-beam"  and  “no-object-in-beam"  signal  strength  values  are  bounded  by  the 
minimum  and  maximum  likely  signal  strengths  for  each  condition.  In  the  “object-in-beam" 
case,  the  function  then  determines  the  World  Model  matrix  coordinates  for  the  first  cell  along 
the  ray  with  a  value  of  1,  and  stores  the  returned  signal  value  in  the  Sonar  Image  matrix  cell 
corresponding  to  the  same  coordinates  of  the  World  Model.  This  ray-tracing  routine  is  repeated 
for  a  discrete  number  of  rays  about  the  entire  swath  of  the  sonar  sensor,  which,  for  the  FLS 
on  the  REMUS,  is  ±45°  of  the  bow.  An  example  of  the  generated  cumulative  sonar  image, 
with  one  object  in  the  tank,  is  shown  in  Figure  4.6.  This  image  is  a  compendium  of  all  of  the 
individual  sonar  images  developed  for  each  sonar  sweep  for  the  entire  trajectory. 

4.4.2  Updating  the  Occupancy  Grid 

The  occupancy  grid  is  initialized  as  a  matrix  with  the  same  dimensions  as  the  given  World 
Model,  with  all  cells  assigned  the  value  of  0.5  (see  Section  2.3).  It  is  then  updated  at  each  point 
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Figure  4.6:  Sample  Cumulative  Sonar  Image 

along  the  trajectory  by  an  OG  update  subroutine  ( OGupdate.m ).  This  function  takes  the  sonar 
image  generated  in  the  previous  subroutine  and  performs  the  Bayesian  inference  described  in 
Section  2.3  and  defined  by  equation  (2.12)  to  update  the  occupancy  probability  of  each  cell.  In 
order  to  obtain  P[rt+ \  ,v(C)  =  Occ]  and  P\rt+\  ,v(C)  =  Emp ],  a  probabilistic  model  of  the  sensor 
must  first  be  built.  This  is  performed  by  yet  another  subroutine  function  ( SensorModelGenera - 
tion.m).  In  this  function,  the  probabilities  that  a  given  sensor  value  is  obtained  given  either  an 
“object-in-beam"  or  “no-object-in-beam"  state  are  modeled  as  continuous  probability  density 
functions  (pdf).  Each  of  these  models  was  generated  based  on  data  from  McChesney4.  The 
pdf’s  for  the  sonar  sensor  model  are  plotted  in  figure  Figure  4.7.  The  occupancy  grid  updater 
processes  each  individual  cell  of  the  sonar  image  and  determines  the  P[rf+i  ^(C)  =  Occ]  and 

4McChesney  utilized  histogram  models  for  sensor  modeling.  Continuous  pdf’s  were  used  in  this  thesis  due  to 
the  lack  of  availability  of  his  histogram  data  and  insufficient  time  for  actual  in-tank  sensor  data  collection.  For  the 
objectives  of  this  thesis,  the  models  used  are  deemed  sufficient  for  proof-of-concept  testing. 
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Figure  4.7:  Sensor  Model  Probability  Distribution  Functions 

P[rt+ 1  |.v(C)  =  Emp]  based  on  the  signal  value  stored  in  the  cell  and  the  probability  value  of  re¬ 
ceiving  this  signal  value  based  on  the  generated  sensor  model.  The  remaining  unknown  variable 
in  equation  (2.12),  =  Occ\{r}t\,  is  the  previous  occupancy  probability  value  of  that  cell. 

The  newly  calculated  occupancy  probability  value  for  this  cell  will  then  be  the  prior  value  for 
the  next  iteration  of  the  Bayesian  inference.  Thus  as  more  sonar  sweeps  across  each  cell  happen, 
the  occupancy  probability  for  each  cell  is  constantly  being  updated,  eventually  converging  on 
the  actual  occupancy  state  of  the  cells.  In  this  way,  the  occupancy  grid  is  constantly  increasing 
in  confidence.  A  sample  occupancy  grid,  based  on  the  data  used  to  produce  the  sonar  image  in 
Figure  4.6  is  shown  in  Figure  4.8. 

4.4.3  Calculating  Information  Gain 

The  same  function  that  updates  the  occupancy  grid  also  calculates  the  incremental  information 
gain  at  each  step.  As  the  occupancy  probability  of  each  cell  is  continually  resolved  by  the 
Bayesian  inference,  our  knowledge  of  the  actual  state  of  each  cell  is  constantly  increasing,  while 
the  information  entropy  is  correspondingly  decreasing.  By  applying  the  concepts  discussed  in 
sections  2.1.2  and  2.2  the  amount  of  change  of  probability  values  of  each  cell  at  each  iteration 
directly  corresponds  to  the  information  gain  achieved  during  that  iteration.  The  information 
gain  is  thus  calculated  during  each  update  as  the  difference  between  the  prior  and  posterior 
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Occupancy  Grid 


Figure  4.8:  Sample  Occupancy  Grid 

occupancy  probabilities  of  each  cell.  The  net  information  gain  over  the  entire  trajectory  is  then 
the  cumulative  sum  of  the  information  gain  achieved  over  every  iteration  for  all  cells. 

4.4.4  Heuristic  Boundaries 

At  the  start  of  the  routine,  the  vehicle  starts  with  no  information  about  its  environment.  Even 
after  the  first  sonar  sweep,  the  only  knowledge  it  has  is  a  low-confidence  probabilistic  estimate 
of  what  lies  in  the  sonar  cone  of  that  initial  sweep.  In  order  to  allow  for  freedom  of  motion  at  this 
beginning  phase  of  the  path,  certain  assumptions  must  be  made  about  the  region  immediately 
surrounding  the  vehicle. 

In  this  vein,  an  initial  “box"  of  free  space  is  established  around  the  vehicle  so  that  it  may 
maneuver  through  unknown  space  until  sufficient  data  has  been  collected  to  permit  normal 
exploration-based  maneuvering  based  on  collected  data.  The  heuristic  bounding  box  assumes 
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that  the  walls  detected  during  the  initial  sonar  sweep  extend  beyond  the  edges  of  the  sonar  cone 
by  a  fixed  amount  sufficient  to  allow  for  an  assumption  of  no  objects  or  obstacles  in  a  path 
traveling  to  the  immediate  left  or  right  of  the  vehicle.  The  forward  looking  sonar  dictates  a  bias 
towards  forward  exploratory  motion,  thus  heuristic  boundaries  astern  of  the  vehicle  originate  at 
the  sternmost  point  of  the  vehicle.  With  these  considerations  in  mind,  one  side  of  the  heuristic 
bounding  box  at  the  vehicle’s  stem  is  defined  as  a  North-South  line  in  the  local  tangent  plane 
(LTP)  frame  originating  at  the  sternmost  point  of  the  vehicle  and  extending  in  the  N-axis  direc¬ 
tion  corresponding  to  that  of  the  N-axis  component  of  the  vehicle  centerline  vector.  The  other 
stem-side  is  similarly  defined  in  the  E-W  direction  in  the  LTP.  The  remaining  two  sides  are  sub¬ 
ject  to  the  shape  of  whatever  is  seen  during  the  initial  sonar  sweep,  and  thus  are  more  difficult 
to  explicitly  define.  It  should  be  noted  that  if  a  secondary  sensory  system  were  utilized,  such  as 
a  side-scan  sonar,  the  augmented  sensory  coverage  to  the  port  and  starboard  side  of  the  AUV 
would  likely  provide  sufficient  information  about  the  adjacent  environment  at  the  start  of  the 
routine  such  that  these  heuristic  boundaries  would  be  unnecessary  for  an  AUV  with  cross-body 
thrusters. 

In  the  case  of  this  thesis,  however,  the  vehicle  starts  in  a  comer  of  the  tank,  so  the  first  assump¬ 
tion  above  is  applied  to  the  walls  of  the  tank  seen  during  the  initial  scan.  The  remaining  two 
(forward)  heuristic  boundaries  are  then  defined  by  extending  the  lines  of  the  walls  seen  by  the 
sonar  beyond  the  sonar  cone.  These  lines  originate  at  the  corner  where  the  right  and  upper  tank 
walls  meet,  extending  beyond  the  initial  sonar  spread  and  terminating  where  the  left  and  lower 
boundaries  intersect  with  the  walls.  The  resulting  boundaries  form  a  box  that  can  be  seen  as 
the  red  dotted  lines  in  Figure  4.9.  These  boundaries  are  used  in  two  ways.  First,  the  occu¬ 
pancy  grid  cells  along  the  heuristic  boundaries  at  the  walls  are  initialized  to  a  value  of  0.75, 
vice  0.5.  Increasing  this  initial  value  reduces  the  possible  information  gain  achieved  by  sensing 
the  walls  within  the  heuristic  bounding  box,  thus  weighting  subsequent  explorations  away  from 
these  wall  sections.  Basically  since  we  are  already  assuming  the  existence  of  the  wall  along 
the  boundaries,  we  want  the  information  gain  accrued  when  sensing  these  walls  to  be  smaller 
than  that  achieved  by  exploring  unknown  areas.  Second,  the  boundaries  at  the  stem  of  the  ve¬ 
hicle  are  used  to  penalize  the  vehicle  for  backing  down  into  unknown  (unexplored)  territory. 
At  the  outset,  the  area  astern  of  the  vehicle  that  lies  beyond  the  stern  heuristic  boundaries  is 
completely  unknown,  and  so  there  obviously  exists  the  possibility  for  obstacles  or  obstructions 
in  those  regions.  Any  generated  trajectory  that  causes  the  vehicle  to  back  down  into  this  space 
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Figure  4.9:  Heuristic  Boundaries,  represented  by  the  red-dashed  lines.  The  blue  rectangle  represents 
the  test  tank  walls;  the  pink  square  is  an  obstacle  in  the  tank;  The  red  box  represents  the  sonar  cone 
of  the  FLS. 

before  it  has  been  explored  is  penalized  proportionally  to  the  extent  by  which  the  stem  violates 
this  boundary.  The  penalty  function  for  this  will  be  discussed  further  in  Section  4.5. 

4.4.5  Obstacle  Avoidance 

An  obstacle  avoidance  subroutine  is  necessary  to  prevent  the  vehicle  from  coming  into  contact 
with  any  obstacles  or  objects  in  the  water.  Since  a  vehicle  with  cross-body  thrusters,  such  as 
the  NPS  REMUS,  is  capable  of  significant  lateral  translation,  it  is  not  sufficient  to  base  the 
avoidance  criteria  simply  on  obstacles  ahead  of  the  vehicle.  Thus  a  method  was  developed  that 
circumscribes  a  circular  “avoidance  perimeter"  about  the  centroid  of  the  vehicle  with  a  radius 
equal  to  the  length  of  the  vehicle.  Each  trajectory  is  then  analyzed  at  each  point  along  the 
path  for  any  intrusion  of  known  object  boundaries  within  this  intrusion  perimeter.  The  penalty 
assigned  to  this  intrusion  is  proportional  to  the  penetration  depth  into  the  perimeter.  The  data 
used  for  “known"  objects  is  sourced  from  the  OG,  meaning  that  only  objects  or  obstacles  that 
have  been  “seen"  by  the  sonar  are  used  in  this  analysis.  This  fact,  combined  with  the  sideslip 
capability  of  the  vehicle,  led  to  the  requirement  for  two  other  subroutines  that  bias  the  trajectory 
into  explored  space,  and  limit  the  amount  of  sideslip  into  unexplored  space.  These  subroutines 
will  be  discussed  further  in  Section  5.2. 
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4.5  Optimization  Scheme  (f mins  ear ch) 

The  routine  now  proceeds  with  the  optimization  portion  of  the  DM-IDVD  process.  A  cost 
function  (CF)  that  calculates  penalties  for  violating  given  constraints  is  calculated,  as  well  as  a 
performance  index  (PI)  of  the  optimized  parameter,  information  gain.  As  stated  previously,  the 
entire  calculative  routine  is  evaluated  within  MAIL AB’ s  fminsearch( )  algorithm.  This  function 
is  a  gradient-free  minimization  routine  that  seeks  to  find  a  local  minimum  of  the  input  function. 
In  the  case  of  the  DM-IDVD  routine,  the  input  function  f(x)  ( trajectory.m ),  consists  of  all 
calculations  examined  thus  far,  and  terminates  with  the  calculation  of  the  proceeding  CF  and  PI. 
The  value  to  be  minimized  is  thus  the  arithmetic  sum  of  these  two  trajectory  function  outputs. 
The  fminsearch  function  also  requires  an  initial  guess  on  the  neighborhood  of  values  for  the 
variable  parameters  in  the  input  function.  It  then  iterates  by  varying  the  values  of  these  variable 
parameters  until  either  the  local  minimum  of  the  function  value  f(x)  maximum  (user-specified) 
number  of  iterations  is  reached.  Values  are  also  specified  for  the  convergence  tolerance  for  both 
f(x)  and  x. 

4.5.1  Cost  Functions  and  Performance  Index 

Penalties  in  the  cost  function  were  initially  specified  for  the  final  time,  yaw  rate,  surge  velocity, 
sway  velocity,  heuristics  violations,  and  allision  avoidance: 


Time:  Penalty  for  exceeding  desired  trajectory  time.  This  desired  time  is  spec¬ 

ified  as  the  time  horizon  TH.  Since  this  isn’t  a  critical  penalty,  a  lower 
relative  weighting  factor  is  assigned  to  it. 


¥■ 

u: 

v: 

Heuristics  Violation: 


Penalty  for  exceeding  max  yaw  rate. 

Penalty  for  exceeding  max  surge  velocity. 

Penalty  for  exceeding  max  sway  velocity. 

Penalty  for  violating  heuristics  boundaries.  This  prevents  backing  into 
the  unknown  region  astern  of  the  vehicle  at  the  start  of  the  trajectory. 


Allision  Avoidance:  Object/obstacle  proximity  penalty.  The  closer  the  path  comes  to  an  ob¬ 
ject  or  obstacle,  the  higher  this  penalty.  This  is  based  on  a  specified 
avoidance  radius  centered  on  the  vehicle  centroid. 
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Table  4.2:  Cost  Function  Components 


Penalty 

Formula 

Time 

Costp  —  (lime  (end)  —  TH)2 

V 

Finey  =  max([0,  (abs(\j/)  -  \j/max)])2 

u 

Fineu  =  max([0,  (abs(u)  -umax)])2 

V 

Finev  =  max([0,  (abs(v)  -vmax)})2 

Heuristics  Violation 

Costa  =  ViolArea* 

Allision  Avoidance 

CostAP  =  AP** 

Notes:  *-ViolArea  is  the  calculated  are  between  the  lower  heuristic  boundary  and  the  arc  swept  by  the  vehicle’s 
stern  in  violation  of  this  boundary.  **-AP  is  the  calculated  by  a  subroutine  that  assigns  a  cumulative  penalty  of 
all  of  the  individual  proximity  violations  at  each  point  along  the  trajectory,  which  are  directly  proportional  to  the 
magnitude  of  the  incursion  of  the  object  into  a  circle  described  about  the  vehicle  centroid  with  a  given  avoidance 
radius. 

Table  4.2  show  the  formulas  used  in  calculating  the  penalties  in  the  cost  function. 

The  performance  index  is  simply  the  inverse  of  the  cumulative  information  gain  for  the  gener¬ 
ated  trajectory,  and  is  calculated  as  Costjc  =  jq-  The  inverse  is  used  to  maintain  consistency 
with  the  other  cost  functions  in  that  it  is  desired  to  minimize  each  within  th e  fininsearch  func¬ 
tion.  Thus  minimizing  the  inverse  of  IG  means  that  IG  will,  in  fact,  be  maximized. 

Each  of  these  CF/PI  factors  is  multiplied  by  a  weighting  factor  and  summed.  This  sum  of  prod¬ 
ucts  is  the  value  that  is  actually  minimized  by  th t  fminsearch  function.  Once  the  minimal  value 
is  found,  the  "optimal"  trajectory  that  produced  it  is  returned,  including  all  state  parameters  ( x , 
y,  and  psi),  as  well  as  the  states  and  controls  calculated  by  the  inverse  dynamics.  These  outputs 
are  the  final  product  of  the  routine-an  optimal  trajectory  that  satisfies  all  constraints  imposed 
on  the  problem  and  maximizes  the  optimization  parameter,  Information  Gain. 
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CHAPTER  5: 

Data  Analysis  and  Findings 


The  routine  described  in  Chapter  4  was  built  and  run  entirely  in  the  MATLAB  computing  envi¬ 
ronment.  All  pertinent  MATLAB  functions  and  scripts  are  contained  in  Appendix  A  on  page  73. 
The  following  sections  discuss  the  numerical  results  of  the  simulation,  as  well  as  the  resulting 
trajectories  for  three  simulation  scenarios  of  the  REMUS  vehicle  in  the  test  tank  with  zero,  one, 
and  two  obstacles  in  the  path  of  the  vehicle.  Constraints  on  surge  (umax),  sway  (ymax),  and  yaw 
rate  (t ]fmax)  were  sourced  from  Doherty  [22].  Initial  guesses  on  the  varied  parameters  and  their 
corresponding  final  optimal  values  for  the  trajectory  generation  runs  are  shown  in  the  corre¬ 
sponding  tables  for  each  set  of  runs.  Prior  to  running  the  full  simulation,  the  initial  guesses 
were  made  empirically  by  running  several  simulation  runs  with  a  reduced  set  of  cost  functions 
(IG,  EV  excluded)  to  validate  the  trajectory  generation  routine.  The  final  values  for  the  varied 
parameters  were  used  as  a  baseline  for  subsequent  complete  optimization  runs.  These  values 
were  adjusted  as  required  until  consistent  results  at  the  end  points  were  observed.  The  simu¬ 
lation  was  then  run  repeatedly,  varying  the  initial  guesses  of  the  variable  parameters  as  well  as 
the  values  for  the  weighting  factors  for  the  cost  functions  and  performance  index.  The  resulting 
state  and  kinematic  parameters  were  then  observed,  and  any  adjustments  required  to  amelio¬ 
rate  or  minimize  constraint  violations  were  made  to  the  weighting  factors.  This  process  was 
repeated  until  a  satisfactory  trajectory  was  obtained. 

The  following  sections  discuss  the  three  scenarios  analyzed:  no  object  in  the  tank,  one  object 
in  the  tank,  and  two  objects  in  the  tank.  Three  representative  runs  from  each  scenario  are 
discussed,  including  the  values  for  the  variable  parameters  and  weighting  coefficients,  as  well 
as  the  results  for  each  run. 

5.1  Scenario  1:  No  obstacles 

The  set  of  runs  for  the  first  scenario  were  made  with  no  obstacles  in  the  tank.  Values  for  the 
pertinent  variable  parameters  for  each  run  are  shown  in  Table  5.1.  The  values  for  the  weighting 
factors  for  each  run  are  shown  in  Table  5.2. 


47 


Table  5.1:  Variabl 

le  Parameter  Initial  Guess  Val 

ues  for  Scenario  1 

1 

2 

3 

4 

5 

6 

7 

8 

9 

10 

Run  1 

10 

1 

0.7854 

0.5 

2.3562 

0.126 

-0.054 

0.0017 

0.0009 

-1.5708  rad 

Run  2 

10 

1 

0.7854 

0.5 

-0.7854 

0.2 

-0.05 

0.017 

0.0009 

-1.5708 

Run  3 

10 

1 

0.7854 

0.5 

-0.7854 

0.126 

-0.054 

0.017 

0.0009 

-1.5708 

Variable  Parameter  Key: 

1.  Virtual  Arc  Length  (xf) 

2.  Magnitude  of  final  acceleration  (m/s2) 

3.  Direction  of  final  acceleration  (radians) 

4.  Magnitude  of  initial  acceleration  (m/s2) 

5.  Direction  of  initial  acceleration  (radians) 

6.  Initial  yaw  acceleration  (y/'r)  (rad/s2) 

7.  Final  yaw  acceleration  ( y/" )  (rad/s2) 

8.  Initial  X" 

9.  Final  X" 

10.  Final  yaw  angle  (radians) 

Table  5.2:  Cost  Function  Weighting  Coefficients  for  Scenario  1 


1 

2 

3 

4 

5 

6 

7 

8 

Run  1 

1 

10 

1 

10 

1 

10 

1 

1 

Run  2 

1 

10 

1 

100 

10 

10 

1 

1 

Run  3 

1 

10 

1 

10 

100 

1 

1 

1 

Weighting  Factor  Key: 

1.  Time 

2.  Yaw  Rate  (\j/) 

3.  Surge  Velocity  (u) 

4.  Sway  Velocity  (v) 

5.  fi 

6.  Heuristics  Violation 

7.  Allision  Avoidance 

8.  Explored  Space 

The  set  of  runs  of  the  full  routine  for  scenario  1  were  done  with  the  vehicle  initially  positioned 
in  the  upper  right  section  of  the  tank.  The  center  of  the  vehicle  was  positioned  one  and  a  half  ve- 
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Figure  5.1:  Scenario  1,  All  Runs,  Initial  Pose 

hide  lengths  from  both  the  top  and  right  (East)  walls  of  the  tank,  and  the  yaw  angle  was  chosen 
to  be  +45°  from  due  North,  thus  pointing  the  vehicle  at  the  corner  of  the  tank,  as  illustrated  in 
Figure  5.1.  This  pose  was  chosen  to  simplify  the  heuristics  assumptions  and  to  put  the  vehicle 
into  the  maximally  constrained  initial  state.  The  end  point  was  then  chosen  to  conform  to  the 
initial  information  space  analysis  discussed  in  Section  4. 1 .  The  probabilistic  analysis  portion  of 
the  routine,  which  included  the  sonar  imaging  subroutines,  Bayesian  inference,  occupancy  grid 
updating,  heuristics  analysis,  and  obstacle  avoidance  subroutines,  was  found  to  be  quite  compu¬ 
tationally  intensive,  causing  very  long  run  times  for  a  sufficient  number  of  iterations.  Because 
of  this,  the  runs  were  initially  performed  without  the  probabilistic  analysis  of  information  gain 
to  allow  for  quick  validation  of  the  basic  setup  of  the  algorithm.  With  no  object  in  the  tank, 
and  no  probabilistic  analysis,  the  routine  initially  generated  unsatisfactory  trajectories.  Without 
the  sonar  imaging  and  resulting  occupancy  grid,  there  was  no  capability  for  allision  avoidance. 
Thus  several  of  the  initially  generated  trajectories  actually  crossed  the  boundaries  of  the  tank 
walls  before  reaching  the  final  point.  Run  1  of  this  set  is  an  example  of  this  result,  as  seen  in  Fig¬ 
ure  5.2.  The  values  for  the  varied  parameters  and  CF  weighting  coefficients  were  then  adjusted 
and  another  set  of  runs  were  made.  The  trajectory  for  Run  2,  illustrated  in  Figure  5.3,  resulted 
in  trajectories  that  remained  within  the  confines  of  the  tank,  but  violated  constraints.  With  the 
trajectories  at  least  now  remaining  within  the  tank,  the  probabilistic  analysis  subroutines  were 
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Figure  5.2:  Scenario  1,  Run  1  Trajectory 


reimplemented  back  into  the  overall  routine,  and  another  set  of  runs  was  performed.  Again, 
the  CF  weighting  factors  were  repeatedly  adjusted  in  an  attempt  to  eliminate  any  constraint  vi¬ 
olations,  however  this  was  not  achieved  in  this  scenario.  Figure  5.4  shows  the  best  trajectory 
achieved  for  the  initial  conditions  in  this  scenario,  and  the  corresponding  occupancy  grid  can 
be  seen  in  Figure  5.5.  Figure  5.6  shows  the  violation  of  the  max  sway  velocity  (v),  while  Fig¬ 
ure  5.7  shows  the  max  yaw  rate  violation  for  this  run.  Subsequent  attempts  to  ameliorate  this 
problem  failed  to  generate  a  fully  satisfactory  trajectory  that  met  all  constraints. 


5.2  Scenario  2:  One  Obstacle 

The  set  of  runs  for  the  second  scenario  were  made  with  a  single  obstacle  in  the  tank  placed  in 
the  nominal  path  of  the  vehicle.  Values  for  the  pertinent  variable  parameters  for  each  run  are 
shown  in  Table  5.3.  The  values  for  the  weighting  factors  for  each  run  are  shown  in  Table  5.4. 


Table  5.3:  Variable  Parameter  Initial  Guess  Values  for  Scenario  2 


1 

2 

3 

4 

5 

6 

7 

8 

9 

10 

Run  1 

10 

1 

0.7854 

0.5 

2.3562 

0.126 

-0.054 

0.0017 

0.0009 

-1.5708 

Run  2 

10 

1 

0.7854 

0.5 

-0.7854 

0.126 

-0.054 

0.0017 

0.0009 

-1.5708 

50 


Figure  5.3:  Scenario  1,  Run  2  Trajectory 


Table  5.4:  Cost 


-unction  Weighting  Coefficients  for  Scenario  2 


1 

2 

3 

4 

5 

6 

7 

8 

Run  1 

1 

10 

1 

10 

1 

10 

1 

1 

Run  2 

1 

10 

1 

10 

100 

10 

1 

1 

Although  no  satisfactory  runs  were  made  in  the  initial  scenario,  an  attempt  was  made  to  continue 
testing  with  a  single  obstacle  in  the  tank,  in  the  hopes  that  further  adjustments  to  the  pertinent 
parameters  combined  with  an  obstacle  in  the  tank  would  drive  down  the  constraint  violations. 
Initial  conditions  on  the  vehicle  position  and  pose  for  the  first  two  runs  of  this  scenario  were 
the  same  as  those  for  the  first  scenario,  illustrated  in  Figure  5.1.  Figure  5.8  shows  an  example 
of  the  resulting  trajectory  with  the  parameters  set  as  indicated  in  Tables  5.3  and  5.4.  As  seen  in 
this  figure,  the  generated  trajectory  starts  in  the  Easterly  direction.  Numerous  attempts  made  to 
modify  the  initial  guesses  for  the  initial  acceleration  angle  to  eliminate  this  behavior  were  fruit¬ 
less.  Trajectories  that  were  forced  to  start  in  the  Northwesterly  direction  continued  to  violate 
kinematic  constraints. 

When  the  initial  guesses  on  the  variable  parameters  were  adjusted  to  force  the  start  of  the  tra¬ 
jectory  away  from  the  comer,  the  algorithm  repeatedly  generated  trajectories  that  caused  some 
degree  of  allision  with  the  obstacle  during  its  transit  to  the  final  point.  Most  of  these  trajectories 
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Figure  5.4:  Scenario  1,  Run  3  Trajectory 

were  predominantly  side-slip  trajectories,  with  little  surge  velocity.  Inspection  of  the  resulting 
penalties  revealed  an  unusual  result:  the  value  of  the  allision  avoidance  penalty  was  zero.  Upon 
viewing  the  generated  sonar  images  and  trajectory  animation,  the  reason  became  clear-in  order 
to  satisfy  constraints  on  time  and  side  slip,  the  generated  trajectories  were  fairly  close  to  linear 
trajectories  between  the  initial  and  final  point,  with  a  small  amount  of  curvature.  However,  due 
to  constraints  on  maximum  yaw  rate,  the  vehicle  was  not  able  to  slew  around  to  the  point  where 
the  object  was  in  the  sonar  field  of  view.  Thus  the  vehicle  was  not  “seeing"  the  obstacle  at  all. 
Figure  5.9  illustrates  one  such  trajectory  in  which  the  stern  of  the  vehicle  actually  allides  with 
the  object  in  the  tank.  Looking  at  the  occupancy  grid  for  this  run  shown  in  Figure  5.10,  it  is 
clear  that  the  vehicle  never  even  saw  the  obstacle.  Two  new  penalties  were  then  added  to  the 
cost  function  to  account  for  this.  First,  a  penalty  on  /3,  the  angle  between  the  yaw  angle  and 
heading,  was  specified.  This  fine  penalizes  the  trajectory  any  time  the  magnitude  of  /3  (|j3|)  is 
greater  than  45°.  This  tries  to  keep  the  heading  of  the  vehicle  within  ±45°  of  the  positive  x-axis 
of  the  vehicle,  thus  favoring  trajectories  that  “drive  into"  the  sonar  cone.  Upon  implementation 
of  this,  however,  it  was  noted  that,  due  to  the  flexibility  of  the  trajectory  in  side-slip  allowed  by 
the  cross  body  thrusters,  the  algorithm  may  still  generate  trajectories  that  travel  in  a  direction 
parallel  to  one  side  of  the  sonar  cone  (i.e.  /3  «  45°)  at  some  point  along  the  trajectory.  Thus 
an  obstacle  might  still  lie  in  the  path  of  the  vehicle,  and  the  sonar  would  never  see  it.  So  a 
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Figure  5.5:  Scenario  1,  Run  3  Occupancy  Grid 

second  additional  cost  function  penalty  was  added  to  favor  trajectories  that  caused  the  centroid 
of  the  vehicle5  to  only  traverse  through  "explored  space"  that  has  been  swept  by  the  sonar.  The 
formulas  used  for  these  penalties  are  given  in  Table  5.5. 

Table  5.5:  Formulas  for  Added  /3  and  Explored  Space  Cost  Function  Penalties 


Penalty 

Formula 

P 

Finep  =  max([0,  (abs(fi)  -  (3max)})2 

Explored  Space 

CostEV  =  1/(1  +EV  ) 

5The  centroid  was  chosen  for  simplicity  of  calculation  within  the  routine.  In  theory,  any  point  aft  of  the  centroid 
along  the  vehicle  centerline  would  be  sufficient  to  impart  a  “moment"  on  the  vehicle  that  would  help  "steer"  it  into 
explored  territory. 
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Figure  5.6:  Scenario  1,  Run  3  vmax  Violation 


Figure  5.7:  Scenario  1,  Run  3  \ j/max  Violation 

Notes:*-EV  is  calculated  by  a  separate  subroutine  that  determines  if,  and  by  how  far,  the  vehicle  centroid  lies 
outside  of  areas  on  the  OG  that  have  values  not  equal  to  0.5,  and  thus  have  been  swept  by  the  sonar. 

As  before,  some  amount  of  adjusting  of  the  weighting  coefficient  for  these  cost  function  terms 
was  performed.  However,  satisfactory  results  were  never  obtained.  The  addition  of  the  two  ad¬ 
ditional  penalties  caused  even  more  “competition"  between  the  components  of  the  cost  function, 
and  finding  the  right  balance  of  weighting  coefficient  values  proved  to  be  incredibly  difficult. 
Continued  adjustments  of  the  initial  guesses  on  the  variable  parameters,  especially  those  on  Xj/ff 
and  the  direction  of  initial  acceleration,  failed  to  sufficiently  ameliorate  this  behavior.  It  seemed 
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Figure  5.8:  Scenario  2,  Run  1  Trajectory 

that  it  may  be  physically  impossible  to  start  with  the  given  initial  pose  of  the  vehicle  and  steer 
around  an  obstacle  to  a  final  point  that  lie  to  the  Southwest  of  the  vehicle,  while  minimizing 
violations  of  the  set  of  physical  constraints  on  u,  v,  and  \j/,  as  well  as  the  heuristics.  Further  ad¬ 
justments  to  the  initial  guesses  on  the  variable  parameters  did  improve  the  situation  somewhat, 
but  the  generated  trajectories  still  ended  up  being  unsatisfactory. 

Careful  consideration  was  given  to  the  reason  for  the  failure  of  the  algorithm  to  generate  satis¬ 
factory  trajectories.  Clues  were  found  when  adjusting  the  weighting  coefficient  on  each  physical 
constraint  during  attempts  to  reduce  violations  of  these  constraints.  For  example,  increasing  the 
coefficient  for  the  sway  velocity  to  bring  v  within  the  vmax  constraints  consistently  caused  a 
larger  violation  of  the  constraint  on  \jf.  This  lent  credence  to  the  hypothesis  that  the  initial  con¬ 
ditions  were  far  too  geometrically  restrictive.  Essentially,  this  meant  that,  with  the  initial  pose 
of  the  vehicle  facing  the  corner,  it  was  practically  physically  impossible  for  the  vehicle  to  tra¬ 
verse  from  the  starting  pose  and  position  to  the  arbitrary  final  point.  Thus  the  initial  conditions 
were  modified  such  that  the  vehicle  was  initially  placed  in  a  pose  at  the  same  starting  coordi¬ 
nates,  but  facing  due  North.  Promising  results  were  obtained  when  starting  from  this  pose,  as 
the  constraint  violations  immediately  became  much  smaller,  and  so  experimentation  was  then 
continued  with  this  new  starting  pose. 
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Figure  5.9:  Scenario  2,  Run  2:  Vehicle  hits  object  in  tank 

5.3  Scenario  3:  Two  Obstacles,  North-facing  Starting  Pose 

With  the  initial  conditions  now  given  by  the  North- facing  starting  pose,  as  shown  in  Figure  5.11, 
satisfactory  results  were  eventually  obtained  for  the  single-obstacle  case.  The  next  step  was  to 
place  two  obstacles  in  the  tank  to  determine  if  the  algorithm  would  generate  a  satisfactory  trajec¬ 
tory  between  both  obstacles  while  optimizing  the  information  gain.  The  same  values  from  the 
immediately  preceding  runs  were  used  for  the  initial  values  for  the  weighting  coefficients  and 
variable  parameters.  Additionally,  the  resulting  animated  plot  of  all  of  the  initial  two-obstacle 
runs  revealed  that  the  vehicle  favored  an  initial  rotation  to  starboard  (clockwise),  leading  to 
many  of  these  large  violations.  No  amount  of  adjustment  of  either  the  weighting  coefficients  or 
variable  parameter  initial  guesses  eliminated  this  behavior.  Again,  the  vehicle  seemed  to  be  too 
tightly  physically  constrained.  The  relatively  short  distance  of  just  a  few  meters  between  the 
start  point  and  end  point,  combined  with  the  extra  obstacle  to  contend  with,  proved  too  restric¬ 
tive  for  the  algorithm  to  handle  given  the  physical  constraints  imposed  on  the  vehicle.  Again, 
another  change  was  made  to  the  boundary  conditions,  this  time  at  the  end  point.  The  time  hori¬ 
zon  was  increased  to  allow  for  a  larger  final  range  of  motion,  and  the  trajectory  end  point  was 
adjusted  accordingly,  towards  the  West  end  of  the  tank.  The  two  obstacles  were  moved  such 
that  they  allowed  for  a  sufficient  impediment  to  travel  between  the  start  and  end  point.  With  this 
revised  setup,  the  simulation  was  run,  and  the  results  were  immediately  clear-greatly  reduced 
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Occupancy  Grid 


Figure  5.10:  Scenario  2,  Run  2  Occupancy  Grid 

physical  constraint  violations,  and  no  allision  with  any  object  or  wall.  An  example  of  an  early 
run  from  this  scenario  can  be  seen  in  Figure  5.12.  As  before,  initial  testing  resulted  in  some 
non-zero  violation  penalties.  Most  significant  in  the  early  runs  were  the  fact  that  the  vehicle 
still  alluded  with  the  upper  obstacle. 

Subsequent  runs  with  adjusted  values  for  the  variable  parameter  initial  guesses  and  weighting 
factors  continued  to  improve  the  resulting  trajectory  and  CF  penalty  values.  An  example  of 
a  better  run  can  be  seen  in  Figure  5.13.  The  vehicle  successfully  avoids  both  obstacles,  but 
constraints  in  both  v  and  y/  were  still  significant,  as  seen  in  Figure  5.14  (v),  and  Figure  5.15 

(VO- 
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Figure  5.11:  Scenario  3,  All  Runs,  Initial  Pose 


Although  the  initial  results  of  runs  using  the  revised  initial  setup,  with  the  Northward  vehicle 
pose  and  increased  trajectory  length,  were  promising,  they  still  did  not  meet  the  requirements 
for  success.  Physical  constraints  were  still  violated,  and  the  performance  index  penalty  was  not 
low  enough  (approx.  4  x  10~4).  As  before,  adjustments  were  made  to  the  weighting  coefficients 
until  satisfactory  results  were  finally  obtained.  Values  for  the  variable  parameter  initial  guesses 
and  weighting  coefficients  can  be  seen  in  Table  5.6  and  Table  5.7. 


Table  5.6:  Variable  Parameter  Initial  Guess  Values  for  Scenario  3 


1 

2 

3 

4 

5 

6 

7 

8 

9 

10 

Run  1 

10 

1 

0.7854 

0.05 

-2.3562 

0.126 

-0.054 

0.0017 

0.0009 

-1.5708 

Run  2 

15 

0.5 

-0.7854 

0.5 

2.3562 

-0.5 

-0.054 

0.0017 

0.0009 

-1.5708 

Run  3 

15 

0.5 

0 

0.25 

2.3562 

-0.1 

-0.054 

0.0017 

0.0009 

-1.5708 

Table  5.7:  Cost  Function  Weighting  Coefficients  for  Scenario  3 


1 

2 

3 

4 

5 

6 

7 

8 

Run  1 

1 

10 

1 

10 

1000 

10 

1 

1 

Run  2 

1 

1 

1 

10 

1 

1 

1 

1 

Run  3 

1 

100 

1 

10000 

1 

1 

0.01 

10 

Figure  5.16  displays  the  final  near-optimal  trajectory  of  the  vehicle.  The  initial  point  is  in¬ 
dicated  by  the  red  star  to  the  upper  right.  As  can  be  seen,  the  vehicle  successfully  navigates 
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Figure  5.12:  Scenario  3,  Run  1  Trajectory 

towards  the  defined  end  point  without  alliding  with  the  fixed  obstacles  or  the  walls  of  the  tank. 
Figure  5.17  shows  the  corresponding  occupancy  grid  for  the  final  successful  run.  Although  this 
trajectory  successfully  navigated  the  trajectory  with  no  constraint  violations  and  an  optimized 
PI,  examining  Figure  5.17  reveals  that  the  sonar  didn’t  actually  “see"  the  lower  obstacle  from  a 
planning  perspective.  From  Figure  5.18,  we  can  see  that  the  vehicle,  constrained  in  yaw  rate, 
was  not  able  to  slew  around  sufficiently  to  sweep  the  sonar  across  the  lower  obstacle.  A  more 
complex  trajectory  with  different  values  on  the  constraints  would  be  required  to  see  this  obsta¬ 
cle.  However,  given  that  the  algorithm  successfully  generated  a  trajectory  that  satisfied  all  of 
the  requirements,  notably  the  constraints  and  performance  index  optimization,  this  trajectory  is 
deemed  to  be  a  valid  near-optimal  trajectory  for  the  given  conditions. 

5.4  Computational  Performance 

One  major  issue  that  was  readily  apparent  during  the  entire  process  was  the  computational  in¬ 
tensity  of  the  routine.  Specifically,  the  subroutines  associated  with  the  sonar  sweep,  occupancy 
grid  updater,  and  obstacle  avoidance  were  especially  intensive  in  both  cpu  cycles  and  memory 
requirements.  Figure  5.19  shows  output  of  the  built  in  MATLAB  profiler  for  the  top  20  func¬ 
tions  used  in  executing  the  entire  algorithm  routine.  The  profiler  profiles  every  line  of  code  as 
it  runs  and  documents  number  of  function  calls,  self  time,  and  overhead  time.  By  inspection,  it 
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is  readily  apparent  that  just  a  few  of  the  subroutine  functions  utilize  a  lions  share  of  the  com¬ 
putational  time,  and  the  impact  is  significant.  In  the  final  run  that  generated  the  satisfactory 
trajectory  detailed  in  this  section,  the  total  computation  time  was  50  seconds6.  As  seen  in  the 
profiler  output,  40%  was  used  for  the  occupancy  grid  calculations  ( OGupdate ),  37%  for  the 
sonar  imaging  (sweep),  and  21%  was  used  in  the  obstacle  avoidance  subroutine  (Avoidance). 


6A11  runs  were  performed  in  MATLAB  7.12.0.635  (R2011a)  32bit,  on  a  PC  running  Microsoft  Windows  XP 
Professional  with  a  2.66GHz  Intel  Core2  Duo  CPU  with  3.25GB  of  RAM.  Attempts  were  made  to  run  the  algorithm 
on  an  NPS  distributed  computing  cluster,  however,  the  MATLAB  license  on  the  cluster  prevents  actual  distributed 
computing,  and  so  minimal  performance  gains  were  actually  realized. 
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Figure  5.14:  Scenario  3,  Run  2  vmax  Violation 
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Figure  5.15:  Scenario  3,  Run  2  y/max  Violation 
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Figure  5.17:  Occupancy  Grid  for  Near-Optimal  Trajectory 


63 


Figure  5.18:  Vehicle  Orientation  at  Several  Points  Along  Final  Near-Optimal  Trajectory 
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Function  Name 

Calls 

Total  Time 

Self  Time* 

Total  Time  Plot 
(dark  band  =  self  time) 

fminsearch 

1 

3051  526  s 

0.251  s 

trajectory 

235  3051  213  s 

1.751  s 

iOGupdatej 

235 

1242.796  s 

1155.515  s 

sweep 

23500  Tl  133.336  s 

1021.028  s 

Avoidance 

23500 

647.725  s 

311.098  s 

bresenham 

10622000 

448.935  s 

448.935  s 

■ 

polwal 

4486742 

87.406  s 

87.390  s 

1 

svm  subs 

940  22.236  s 

0  080  s 

svm  subsxnupadsubs 

940 

21.130  s 

0.046  s 

svm  subs  >t rvF u  n  ct  i  o  n  H  a  n  d  1  e 

940 

21.084  s 

0.345  s 

mupadmex  (MEX-file) 

3978  [  19.255  s 

19.255  s 

1 

svm  char 

940 

16.293  s 

0.125  s 

InitMatrices 

235  2  745  s 

2.745  s 

svm  subs>qetUnknowns 

940 

1.872  s 

0.078  s 

svm.subs>makeFhandle 

940 

1.157  s 

0  750  s 

svm  subs>qetVars 

940 

1.026  s 

0.110  s 

RefFuncs 

1  0  969  s 

0.078  s 

svm.findsvm 

940 

0  916  s 

0.094  s 

svm  subs>evalableY 

940 

0  872  s 

0  201  s 

cell  setdifF 

1880  0.735  s 

0  296  s 

Figure  5.19: 


MATLAB  Profiler  Output  for  Final  Optimal  Trajectory  Run 
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CHAPTER  6: 

Conclusions  and  Recommendations 


6.1  Conclusions 

As  seen  in  Chapter  5,  valid  results  were  obtained  for  the  problem  of  generating  an  optimal 
trajectory  that  not  only  optimized  the  key  parameter  of  interest,  Information  Gain,  but  also 
satisfied  all  constraints  imposed  on  the  trajectory,  such  as  kinematic  and  obstacle  avoidance 
constraints.  However,  the  stated  goal  of  generating  these  optimal  trajectories  in  real  time  may  be 
jeopardized  by  the  computational  intensity  of  the  algorithm.  As  discussed  in  Section  5.4,  just  a 
few  MATLAB  routines  occupied  a  significant  share  of  the  computation  time.  These  subroutines 
are  key  to  the  sonar  imaging,  probabilistic  analysis  of  information  gain,  and  obstacle  avoidance. 
As  each  of  these  routines  performs  numerous  calculations  sequentially  on  individual  cells  of 
large  matrices,  some  computational  rigor  is  expected.  An  important  aspect  to  consider,  however, 
is  that  much  of  the  intensive  computation  being  performed  by  MATLAB  in  the  simulation 
environment  will  actually  be  done  by  dedicated  hardware  on  the  REMUS  vehicle.  For  example, 
the  sonar  imaging  that  consumed  so  much  computation  time  in  MATLAB  will  be  performed  by 
the  Blueview  FLS  and  it’s  dedicated  image  processing  hardware.  This  alone  would  drastically 
reduce  the  load  on  the  algorithm  itself.  Similar  gains  would  likely  be  achieved  by  other  aspects 
of  the  hardware  on  the  vehicle,  especially  given  that  compiled  code  runs  orders  of  magnitude 
faster  than  MATLAB  interpreted  code. 

A  key  observation  made  during  the  algorithm  test  and  evaluation  phase  of  this  thesis  was  the 
sensitivity  of  the  trajectory  to  the  adjustments  made  to  the  weighting  coefficients  and  variable 
parameter  initial  guesses.  Many  adjustments  had  to  be  made  to  several  of  these  parameters 
in  order  to  generate  physically  valid  trajectories  for  each  scenario.  In  some  cases,  adjusting  a 
weighting  coefficient  to  minimize  one  penalty  resulted  in  an  increase  in  another  penalty.  An 
example  of  this  is  when  adjustments  made  to  reduce  the  /3  violation  penalty  resulted  in  higher 
yaw  rates,  increasing  the  \j/max  violation.  Further  thesis  research  is  required  to  investigate  the 
interplay  between,  and  sensitivity  of,  each  of  the  variable  parameters  and  weighting  coefficients. 

Another  interesting  observation  is  that  the  generated  trajectories  were  often  not  intuitive  in  that 
they  were  frequently  non-linear  and  deviated  significantly  from  the  relatively  simple  trajectories 
expected  for  reactive  obstacle  avoidance.  As  discussed  in  the  thesis,  the  DM-IDVD  routine 
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generates  full  sets  of  candidate  trajectories  by  varying  kinematic  parameters  at  the  boundaries 
of  the  trajectory,  such  as  speed,  acceleration,  etc.  This  feature,  combined  with  the  fact  that 
the  coordinates,  yaw  angle,  and  speed  factor  of  the  trajectories  are  represented  by  reference 
functions  combining  high  order  polynomials  and  trigonometric  functions,  produces  these  that 
are  far  more  flexible  and  complex,  both  spatially  and  kinematically. 

In  conclusion,  this  thesis  successfully  proved  the  concept  of  utilizing  the  Direct  Method  of 
Inverse  Dynamics  in  the  Virtual  Domain  to  calculate  information-optimal  trajectories  based  on 
a  probabilistic  analysis  of  information  gain.  However,  the  viability  of  utilizing  this  method  for 
real-time  trajectory  generation  was  not  established.  Time  constraints  precluded  further  code 
optimization  and  compilation  into  executable  code  to  be  run  on  the  vehicle,  and  so  in-tank 
testing  on  the  REMUS  vehicle  itself  was  never  achieved. 

6.2  Recommendations  for  Future  Work 

As  discussed  in  the  conclusions,  there  may  well  be  significant  opportunity  for  performance 
improvements  by  optimizing  the  MATLAB  code  used  to  program  the  routine.  Careful  scrutiny 
might  be  given  to  specific  functions  and  subroutines  within  the  algorithm  to  determine  where 
and  how  code  optimizations  may  be  made  to  improve  the  efficiency  of  the  routine.  Hardware- 
in-the-loop  simulation  may  also  significantly  speed  up  the  computations  in  the  algorithm  as 
well.  Specifically,  the  author  recommends  examining  alternative  methods  for  the  following 
subroutines/functions:  ray  tracing  (performed  in  both  the  sonar  imaging  and  allision  avoidance 
subroutines);  occupancy  grid  cell  probabilistic  calculations  (specifically  where  the  sensor  signal 
probabilities  are  evaluated  from  the  sensor  model  probability  density  functions);  and  symbolic 
math  manipulations,  specifically  wherever  the  MATLAB  polyval()  functions  is  utilized. 

Following  any  required  code  optimizations,  the  obvious  next  step  should  be  to  compile  the 
algorithm  into  computer  code  compatible  with  the  computer  resources  on  the  REMUS  vehicle 
and  perform  actual  in-tank  testing  in  the  CAVR  AUV  test  tank.  Following  the  completion  of 
successful  in-tank  testing,  open  water  testing  would  then  be  needed  to  ascertain  the  viability  of 
the  routine  in  the  presence  of  disturbances. 

Concomitant  with  on-vehicle  testing  of  the  algorithm  would  be  implementation  of  methods 
required  to  reduce  real-world  positional  uncertainty  caused  by  sleep-state  drift  and  inherent 
sensor  error  and  noise.  As  methods  for  performing  this  task,  such  as  Kalman  filters,  are  already 
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well  known  and  developed,  augmenting  the  algorithm  developed  by  this  thesis  with  a  positional 
uncertainty  reduction  scheme  should  be  fairly  trivial. 
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APPENDIX  A: 
MATLAB  Code 


A.l  Main  Function:  Main_opt.m 


%+^r^++*******^r+Jr+******^  +  ********^r^++********+*  +  *******+  +  Jnt******  +  ++  ******* 

%  MAIN_OPT.M 

%  THIS  IS  THE  MAIN  SCRIPT.  RUN  THIS  TO  PERFORM  THE  FULL  SIMULATION. 

O. 

O 

%  Written  by:  LT  Adam  Wiseman 

%  All  code  was  written  by  the  author  with  the  exception  of  bresenham.m  and 
%  the  code  for  generating  the  animated  plot.  Credits  for  bresenham.m  are 
%  inside  that  script,  and  credit  for  the  animated  plot  code  and  the 
%  associated  scripts/functions  code  goes  to  Dr.  Les  Carr  of  the  Math  Dept. 

%  at  NPS . 

o, 

o 

%  Last  Edited:  23  March  2012 

%****+*****+*  +  ***********  +  ***^r****ir********^+*  +  *********^*  +  ********+*+***** 

%%  Prep — clear  workspace,  declare  global  variables 
clear  all 
close  all 
clc 

profile  on  —history 

global  L  D  W  H  R  psi_dot_max  u_max  v_max  dt  TH  M  N 
global  xinit  yinit  psii  beta_max  %psif 
global  OG  Snrlmg 

global  AnimFlag  ObsFlag  ProbFlag  x  y  VARS 


%%  Switches — turn 
AnimFlag=l;  % 

ProbFlag=l;  % 

o_ 

o 

ObsFlag=l;  % 


certain  features  on/off 

Set  to  0  to  disable  display  of  the  REMUS  animation 
Set  to  0  to  run  routine  without  probabilistic  analysis 
(speeds  up  trajectory  validation  process) 

Set  to  0  to  disable  obstacles  in  tank 


%%  Vehicle  and  Tank  Dimensions 
L=2.75;  %  vehicle  length  in  meters 
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D=0 .19; 
W=2  0 ; 
H=10 ; 


%  vehicle  diameter  in  meters 
%  tank  length  in  meters 
%  tank  width  in  meters 


R=2  *H; 


%  This  is  used  to  control  size  of  tank  matrix  oversize  and 
%  sonar  beam  range.  It  was  originally  set  to  the 
%  range  of  the  sonar,  but  this  resulted  in  overly  large  data 
%  matrices,  and  was  thus  reduced  in  length,  with  no  detriment 
%  to  the  overall  routine  within  the  constraints  of  the  test 
%  tank  dimensions.  Future  testing  in  larger  environments  will 
%  require  this  to  be  set  back  to  the  sonar  range. 


M=10* (2*R+H) ;  %  Row  space  for  model  grids/matrices 

N=10* (2*R+W) ;  %  Column  space  for  model  grids/matrices 

%  Note:  The  multiplication  factor  of  10  allows  for  finer  grids  in  the 
%  sonar  image  and  occupancy  grid,  and  minimizes  rounding  errors  caused  when 
%  rounding  non— integer  coordinate  values  to  integer  values  for  matrix  cell 
%  addressing.  This  multiplication  factor  is  also  found  in  some  of  the 
%  subroutines  that  require  coordinate— to— mat rix  cell  index  translation. 


%%  Initialization  of  parameters 
%*****  Time  Horizon  (s)  ***** 

TH=15;  %  Nominal  time  of  trajectory  from  start  point  to  end  point. 

%**************************** 

dt=.l;  %  Time  step  (s) 

rf=5;  %  Refinement  factor  for  refining  data  mesh.  This  simply 

%  decreases  the  step  size  of  each  maneuver.  Raise  this 
%  value  to  reduce  the  step  size. 

IG=[];  %  Initialize  Information  Gain  (for  computational  speed) 

%  Constraints 


dp=14 .5; 

o_ 

o 

Max  yaw  rate  (deg/sec) 

psiTH=TH*dp; 

o, 

o 

Max  yaw  over  time  horizon  (deg) 

psi_dot_max  =  dp*pi/180; 

o, 

o 

maximum  yaw  rate,  rad/ s 

v_max  =  . 5 ; 

o, 

o 

maximum  sway  velocity,  m/ s 

u_max  =  2.88; 

o, 

o 

surge  velocity  in  m/s 

beta_max=45*pi/180 ;  %  absolute  value  of  max  desired  beta 

%  (to  keep  trajectory  within  sonar  cone) 


%*  *  *  * INITIAL  PARAMETER  VALUES!!!**** 

xinit=W— 3*L/2 ;  %  x— coordinate  of  origin  of  distribution  circle 
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%  xinit=190; 


%  For  testing  purposes  only 

%  y— coordinate  of  origin  of  distribution  circle 
%  For  testing  purposes  only 


yinit=H— 3*L/ 2  ; 

%  yinit=80; 


psiinitdeg  =  0;  %  Initial  Heading  in  degrees 

psiinit  =  pi/180*psiinitdeg; %  Initial  Heading  in  radians 


%*  *  *  *FINAL  PARAMETER  VALUES !  !  ! **** 


xf in=3 ; 
yfin=5; 
psi_d=— 45; 

psif in=psi_d*pi/ 180 ; 


%  Final  yaw  angle  in  degrees 
%  Final  yaw  angle  in  radians 


%%  Heuristic  Boundaries 

%  Note:  HB  are  completely  artificially  generated  here.  In  the  future,  it 
%  will  most  likely  be  necessary  to  generate  these  on  the  fly  based  on 
%  initial  sonar  sweeps. 

if  ProbFlag 

global  BB  UB  RB  LB  RBx  LBx  UBy  BBy 

%  Upper  Heuristic  Bound  (top  wall) 

UB=R; 

%  Lower  (Bottom)  Heuristic  Bound 
BBy=yinit— L/2 ; 

BB=H+R— BBy ; 

%  Right  Heuristic  Bound  (Right  wall) 

RB=W+R; 

%  Left  Heuristic  Bound 
LBx=xinit— L/ 2 ; 

LB=LBx+R; 

%  Bounding  box  corners  for  plotting  purposes 
UBy=H+R— UB ; 

RBx=RB— R; 

xHCorners= [LBx  LBx  RBx  RBx  LBx]  ; 
yHCorners= [BBy  UBy  UBy  BBy  BBy]  ; 


end 
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120 
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124 

125 
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127 

128 

129 

130 

131 

132 

133 

134 

135 

136 

137 

138 

139 

140 

141 

142 

143 

144 

145 

146 

147 

148 

149 

150 

151 

152 

153 
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156 


Initialize  Sensor  model 


9-  2- 
o  o 

if  ProbFlag 

Sens orModelGene rat ion; 

end 


%%  Object 

global  xl  xr  yl  yu  x!2  xr2  yl2  yu2 
%  Object  1 


<x> 

II 

1 — 1 

X 

o. 

o 

Left  ! 

boundary  i 

of 

object 

xr=7  ; 

o, 

o 

Right 

boundary 

of 

object 

yl=7; 

o, 

o 

Lower 

boundary 

of 

object 

00 

II 

p 

>1 

o, 

o 

Upper 

boundary 

of 

object 

%  Object  2 


xl2=8 ; 

o, 

o 

Left  ! 

boundary  o 

f 

object 

xr2=9; 

"o 

Right 

boundary 

of 

object 

yl2=2; 

o, 

o 

Lower 

boundary 

of 

object 

yu2=3 ; 

o. 

o 

Upper 

boundary 

of 

object 

%%  DIRECT  METHODS  OPTIMIZATION  (IDVD) 

%-k-k-k-k-k-k-k-k-k-k-kiz-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-kiz-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k 

%  Everthing  below  this  is  part  of  the  DM  process. 

%-k-k-k-k-k-k-k-k-k-k-kiz-k-k-k-k-k-k-k-k-k-kiz-k-k-k-k-k-k-k-k-k-kiz-k-k-k-k-k-k-k-k-k-kiz-k-k-k-k-k-k-k-kiz-kiz-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k 


o_  o, 
o  o 

Defining  optimization  problem 

global  Fine_v 

Cost_T  Fi 

ne_YawRate 

Fine_u  Fine. 

_beta 

global  wT  wY  wu  wv  wb 

wT 

=  leO ; 

o_ 

o 

weighting 

coefficient 

for 

time  of  arrival 

wY 

=  le2  ; 

o, 

o 

weighting 

coefficient 

for 

yaw  rate 

wu 

=  leO ; 

o_ 

o 

weighting 

coefficient 

for 

surge  velocity  u 

wv 

=  le3  ; 

o, 

o 

weighting 

coefficient 

for 

sway  velocity  v 

wb 

=  leO ; 

o, 

o 

weighting 

coefficient 

for 

beta 

if 

ProbFlag 

global  wG 

wH  wA  wE 

Cost_EV  Cos 

t_H  Cost_IG 

Cost 

_AP  avrad 

wG  =  lei; 

o_ 

o 

weighting 

coefficient 

for 

Information  Gain 

wH  =  leO; 

o, 

o 

weighting 

coefficient 

for 

Heuristics  Violations 

wA  =  1 e  0 ; 

o_ 

o 

weighting 

coefficient 

for 

allision  avoidance 

wE  =  lei; 

o, 

o 

weighting 

coefficient 

for 

unexplored  region  penalty 
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158 

159 

160 

161 

162 

163 

164 

165 

166 

167 

168 

169 

170 

171 

172 

173 

174 

175 

176 

177 

178 

179 

180 

181 

182 

183 

184 

185 

186 

187 

188 

189 

190 

191 

192 

193 

194 

195 

196 

197 


avrad=L; 


Obstacle  Allision  Avoidance  radius 


end 


%%  Setting  the  boundary  conditions 

global  vxi  posxi  vxf  posxf 

global  vyi  posyi  vyf  posyf  lami  lamf 


posxi 

=  xinit; 

posxf 

=  xfin; 

vxi  = 

0; 

vxf  = 

0; 

%  accxi  =  0; 

%  accxf  =  0; 

%  betai=atan2 ( 
psii=psiinit ; 
lami=l ; 


posyi  =  yinit; 
posyf  =  yf in; 
vyi  =  0; 
vyf  =  0; 

accyi  =  0; 
accyf  =  0; 

:i,  vyi )  — psiinit  ; 
%  psif=psifin; 
lamf=l ; 


%  initial  position 
%  final  position 

%  components  of  initial  velocity 
%  components  of  final  velocity 

%  components  of  initial  acceleration 
%  components  of  final  acceleration 
betaf=atan2 (vxf , vyf )— psifin; 

%  initial  and  final  yaw  angles 
%  initial  and  final  lambdas 


%%  Guessing  on  the  varied  parameters 


guess (1) =15; 
guess  (2) =0 . 5; 
guess (3) =— 0*pi/4; 
guess ( 4 ) =0 . 25 ; 
guess (5)=3*pi/4; 
guess  ( 6) =— . 1 ; 
guess ( 7 ) =— 0 .  054 ; 
guess ( 8 ) =.0017; 
guess  ( 9) = . 000 9; 
guess  (10)=— pi/ 2; 


%  virtual  arc  length  (tauf) 

%  magnitude  of  final  accel  (accxf) 

%  direction  of  final  accel  (accyf) 

%  magnitude  of  initial  accel  (accxf) 
%  direction  of  initial  accel  (accyf) 
%  initial  psi  double  prime  (psippi) 

%  final  psi  double  prime  (psippf) 

%  initial  lambda— dbl— prime 
%  final  lambda— dbl— prime 
%  final  psi 


%%  Define  the  Reference  Function  Coefficients 

RefFuncs;  %Note:  Doing  this  the  other  way  by  including  the  coefficient 
%vectorss  in  the  trajectory  function  causes  an  error.  Keeping 
%it  this  way  for  now.  However,  some  computational  optimization 
%may  be  acheived  by  including  the  output  of  this  function 
%directly  into  the  trajectory. m  script  to  eliminate  the 
%symbolic  math  computations  performed  in  this  function.  The 
%coefficient  vectors  have  been  left  in  trajectory. m  but 
%commented  out  to  allow  for  future  implementation.  If  the 
%other  method  is  used,  this  line  may  be  commented  out. 
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%%  Calling  the  optimization  routine 

maxiter=1000;  %  maximum  number  of  iterations  within  fminsearch 

maxfuns=5*maxiter ;  %  maximum  number  of  function  evaluations 

%  Setting  the  options  for  fminsearch: 

opt=optimset ( 1  Display '  ,  'iter',  ' TolX ' , le— 2,  ' TolFun '  ,  le— 2,  .  .  . 

' Maxi ter  1 , maxiter,  1 MaxFunEvals ' , max fun s ) ; 
t  =  cputime;  %  Used  to  calculate  and  display  actual  computational  time 

^^it^^^isisis-k'k-k'k-k'k'k'k-k-k-k-k'k-k'k-k'k'k-k-k-k'k-k'k-k'k'k'k'k-k-k-k'k-k'k-k'k'k'k'k-k-k-k'k-k'k-k'k'k'k-k-k-k-k'k-k'k-k'k'k'k-k-k-k-k'k 

%  Calling  the  fiminsearch  optimization  function: 

[guess_opt , fval, exit flag]  =  fminsearch ( 'trajectory ' , guess, opt ) ; 

%isit^^^is^isisisisisit'k'k'k-k-k-k-k-k-k-k-k'k'k-k-k-k-k-k-k-k'k'k'k'k-k-k-k-k-k'k-k'k'k'k'k-k-k-k-k-k'k-k'k'k'k-k-k-k-k-k-k'k-k'k'k'k-k-k-k-k'k 


%  calculating  and  displaying  computational  time: 
time_e lapsed  =  cputime— t; 

fprintf (' fminsearch  Elapsed  Time:  %4.2g  minutes . \n\n ', t ime_elapsed/ 60 ) 
profile  off 

%%  Displaying  cost  functions  and  penalties 


if  ProbFlag 

fprintf ( '  Information  Gain  Cost  function 
fprintf ( '  Heuristics  Violation  Cost  function 
fprintf ( '  Allision  Avoidance  Cost  function 
fprintf ( '  Unexplored  Territory  Cost  function 


%6 . 2g\n ' , Cost_IG) 

:  %6 . 2g\n ' , Cost_H) 

:  %6 . 2g\n ' , Cost_AP ) 

:  %6 . 2g\n ' , Cost_EV) 


end 

fprintf ( '  Time  Cost  function 
fprintf ( '  Penalty  in  sway  v 
fprintf  (  '  Penalty  in  surge  u 
fprintf  (  '  Penalty  in  YawRate 
fprintf ( '  Penalty  in  beta 


:  %6 . 2g\n ' , Cost_T) 

%6 . 2g\n ' , Fine_v) 

%6 . 2g\n ' , Fine_u) 

:  %6 . 2g\n ' , Fine_YawRate) 

%6 . 2g\n\n 1 , Fine_beta) 


%%  Displaying  optimal  parameters 


%  Display  initial  guesses  on  varied  parameters 

disp  (' Initial  Guesses  on  Values  of  Varied  Parameters:') 

fprintf ('Arc  length  =  %6 . 2f\n ', guess (1) ) 

fprintf (' Final  x,y— accel  magnitude  =  %6 . 2f \n ' , guess (2 ) ) 

fprintf  (' Final  x.y— accel  direction  =  %6.2f  deg\n ' , guess (3) *180/pi) 

fprint f  (' Initial  x,y— accel  magnitude  =  %6 . 2f \n ' , guess (4 ) ) 
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fprintf  (' Initial  x,y— accel  direction  =  %6.2f  deg\n ’ ,  guess (5) *180/pi) 

fprintf (' Initial  psi  accel  =  %6 . 2f \n ' , guess ( 6) ) 

fprint f (' Final  psi  accel  =  %6 . 2f \n ' , guess (7 ) ) 

fprint f (' Initial  lambda  dbl  prime  =  %6 . 2f \n ' , guess (8) ) 

fprint f (' Final  lambda  dbl  prime  =  %6 . 2f \n 1 , guess ( 9)  ) 

fprint f (' Final  psi  =  %6 . 2f\n\n ' , guess (10) *180/pi) 

%  Display  final  values  of  varied  parameters 

disp  (  ' Final  Values  of  Varied  Parameters:') 

fprintf ('Arc  length  =  %6 . 2f \n ' ,  guess_opt ( 1 ) ) 

fprint f  (' Final  x,y— accel  magnitude  =  %6 . 2f \n ' , guess_opt (2 ) ) 

fprint f  (' Final  x.y— accel  direction  =  %6.2f  deg\n ' ,  guess_opt (3) *180/pi) 

fprint f (' Initial  x,y— accel  magnitude  =  %6 . 2f \n ' , guess_opt (4 ) ) 

fprint f (' Initial  x,y— accel  direction  =  %6.2f  deg\n ' , guess_opt (5) *180/pi) 

fprint f  (' Initial  psi  accel  =  %6 . 2f \n ' ,  guess_opt ( 6) ) 

fprint f (' Final  psi  accel  =  %6 . 2f \n ' , guess_opt (7 ) ) 

fprint f  (' Initial  lambda  dbl  prime  =  %6 . 2f \n ' , guess_opt ( 8 ) ) 

fprint f (' Final  lambda  dbl  prime  =  %6 . 2f \n ' , guess_opt ( 9) ) 

fprintf (' Final  psi  =  %6 . 2f \n\n ' , guess_opt ( 10 ) *180/pi) 

%%  Plotting  the  results 
PlotResults 

%%  END 
profile  off 

%  The  following  line  saves  all  variables  in  the  workspace  to  a  .mat  file 
%  for  future  reference.  Change  this  each  run  to  avoid  overwriting 
%  previous  saves 

%  save  (  '  Run_3— 22_l_incwv_f  acingN— fminsearch— lOOOiter  '  ) 

A.2  Trajectory  Generation  and  Optimization  Function:  trajectory. m 

function  PI=tra jectory (guess) 

%%  This  function  computes  states  and  controls  for  the  current  guess. 

%  It  is  the  input  function  for  fminsearch,  and  cannot  be  run  alone.  It  is 
%  the  function  to  be  optimized  by  fminsearch. 
global  VARS  ProbFlag 

global  vxi  posxi  vxf  posxf  lami  lamf 

global  vyi  posyi  vyf  posyf  psii  %  psif  betai  betaf 
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global  x  y  time  V  u  v  psi  Psi  tau  lam  beta  psi_dot 
%%  Initialize  Matrices/Grids — see  notes  in  script 


InitMatrices ; 

%%  Current  values  of  varied  parameters 

tauf 

=  guess  ( 1 ) ; 

%  virtual 

arc  length 

accxf 

=  guess (2) *cos (guess (3) ) ; 

%  final  x 

accel 

accyf 

=  guess (2) *sin (guess  (3) ) ; 

%  final  y 

accel 

accxi 

=  guess (4) *cos (guess (5) ) ; 

%  initial 

x  accel 

accyi 

=  guess ( 4 ) *sin (guess  ( 5 ) ) ; 

%  initial 

y  accel 

psippi 

=  guess  ( 6) ; 

%  initial 

beta  double  prime 

psippf 

=  guess  (7) ; 

%  final  beta  double  prime 

lamppi 

=  guess  (8) ; 

%  initial 

1 amb da— db 1— p  rime 

1 ampp  f 

=  guess  ( 9) ; 

%  final  lambda— dbl— prime 

psif 

=  guess  (10) ; 

%  final  psi 

%%  Defining  parameters  in  M/NN  nodes  in  the  virtual  domain 

global  a  apsi  alam  Mp  Np  01 
%  (x,y)  Reference  Function  Coefficients 

%  Note:  Putting  the  coefficient  matrices  in  here  gives  me  an  error,  so  I'm 
%  keeping  the  RefFuncs  subroutine  for  now.  If  the  errors  can  be  resolved, 

%  it  would  be  computationally  quicker  to  eliminate  the  symbolic  math 

%  computations  in  RefFuncs  and  simply  use  the  coefficient  vectors  below. 

%  syms  tf  xO  xpO  xppO  xf  xpf  xppf 
%  a  =  [xO; 

%  (— xppO/3  —  xppf/6)*tfA2  —  xO  +  xf; 

%  (tf A2*xpp0 ) /2 ; 

%  — tf A2*  (xppO/6  —  xppf/6); 

%  ( (xppO  +  xppf ) *t f A2 ) /  ( 4 *pi )  +  (  (2*xp0  —  2*xpf ) *t f ) /  ( 4 *pi ) ; 

%  ((xppO  —  xppf ) *tf A2 ) / (24*pi)  +  ((6*xp0  +  6*xpf ) *tf ) / (24*pi)  +  ... 

( 12*x0  -  12*xf ) /  (24*pi) ] ; 

o_ 

o 

ax=subs (a,  { ' xO  ’  ,  : xpO ' ,  ' xppO  '  ,  ' xf ' ,  ' xpf ' ,  ' xppf ' ,  ' tf ' } ,  . .  . 

{posxi, vxi, accxi, posxf , vxf , accxf , tauf } ) ; 
ay=subs (a, { ' xO ' , ' xpO ' , ' xppO ' , ' xf ' , ' xpf ' , ' xppf ' , ' tf ' } , . . . 

{posyi, vyi, accyi , posyf, vyf, accyf , tauf } )  ; 
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%  (psi)  Reference  Function  Coefficients 
%  syms  psiO  psipO  psippO  psiff  psipf  psippff  tf 
%  apsi=[psiO 
%  psipO*tf; 

%  (psippO*t f A2 ) /2 ; 

%  (psippff/2  —  ( 3*psipp0 ) /2 ) *t f A2  +  (— 6*psip0  —  4*psipf ) *tf  —  ... 

10*psi0  +  10*psiff; 

%  (  ( 3*psipp0 )  /2  —  psippff )  *tfA2  +  (8*psip0  +  7*psipf)*tf  -I-  ... 

15*psi0  —  15*psiff; 

%  (psippff/2  —  psippO/2 ) *t f A2  +  (— 3*psip0  —  3*psipf ) *tf  —  6*psi0  ... 

+  6*psiff ] ; 

ap=subs (apsi, { ' psiO ' ,  'psipO ' ,  ' psippO '  , 1 psiff 1 ,  'psipf’, ' psippff '  , ' t f ' } , . . . 

{psii, 0, psippi, psif , 0, psippf , tauf } )  ; 

%  (lambda)  Reference  Function  Coef f iecients 
%  syms  lamO  lampO  lamppO  lamff  lampff  lamppff  tf 
%  alam=[lamO; 

%  lampO *tf; 

%  (lamppO*tf A2) /2; 

%  (lamppff/2  —  ( 3*lampp0 ) /2 ) *t f A2  +  (—  6*lamp0  —  4*lampff ) *tf  —  ... 

10*lam0  +  10*lamff; 

%  ( ( 3*lampp0 ) /2  —  lamppff ) *tfA2  +  (8*lamp0  +  7*lampff)*tf  +  ... 

15*lam0  —  15*lamff; 

%  (lamppff/2  —  lamppO/2 ) *t f A2  +  (—  3*lamp0  —  3*lampff ) *tf  —  ... 

6*lam0  +  6 * lamff ] ; 

a 1= subs (alam, { ' lamO ' , ' lampO ' , ' lamppO ' , ' lamff ' , 1 lampff ' , ' lamppff ' , ' t f ' ) , . . . 

{ lami, 0, lamppi, lamf , 0, lamppf ,  tauf ) ) ; 

%  If  using  the  commented  out  coefficient  vectors  above,  uncomment  the 
%  following  lines  as  well: 

%  Mp=length (a) ; 

%  Nb=length (apsi)  ; 

%  01=length (alam) ; 

for  i=l : Mp— 2 

cx ( i ) =ax (Mp— 1— i ) ; 
cy (i) =ay (Mp-l-i) ; 
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end 

for  i=l : Np 

cp (i) =ap (Np+l-i)  ; 

end 

for  i=l : 01 

cl (i) =al (01+1— i) ; 

end 

taubar=linspace  (0, 1) ;  %  taubar=tau/tauf  (See  report  for  why) 

%  Evaluate  complete  ref  funcs  to  obtain  vectors  of  state  parameters: 
x=polyval (cx, taubar) +ax (5) *sin (pi*taubar ) +ax (6) *sin ( 2 *pi* taubar) ; 
y=polyval (cy, taubar) +ay (5) *sin (pi* taubar) +ay (6) *sin ( 2 *pi* taubar) ; 
psi=polyval (cp, taubar) ; 
lam=polyval (cl, taubar) ; 

NN=length (x) ; 

%%  Defining  parameters'  derivatives  in  NN  nodes  in  the  virtual  domain 
cx_prime=cx . * [3 1 : 0] *eye (4,3) ; 
cy_prime=cy . * [3 1 : 0] *eye (4,3) ; 

x_prime=polyval (cx_prime, taubar) +ax (5) *pi*cos (pi* taubar) +ax ( 6) *2*pi* . . . 

cos  (2*pi*taubar) ; 

y_prime=polyval (cy_prime, taubar) +ay (5) *pi*cos (pi* taubar) +ay ( 6) *2*pi* . . . 

cos (2*pi*taubar) ; 

x_prime=x_prime/tauf ; 
y_prime=y_prime/tauf ; 

cx_dblprime=cx . * [ 6  2  0  0]*eye(4,2); 
cy_dblprime=cy . * [ 6  2  0  0]*eye(4,2); 

x_dblprime=polyval (cx_dblprime, taubar)— ax (5) *piA2*sin (pi* taubar ) — . . . 

ax ( 6) * (2*pi) A2*sin ( 2 *pi* taubar) ; 

y_dblprime=polyval (cy_dblprime, taubar)— ay (5) *piA2*sin  (pi* taubar )  — .  .  . 

ay (6) * (2*pi) A2*sin (2*pi*taubar) ; 

x_dblprime=x_dblprime/tauf A2 ; 
y_dblprime=y_dblprime/tauf A2 ; 

cp_prime=cp. * [5 :— 1 : 0] *eye (6,5) ; 

psi_prime=polyval (cp_prime, taubar ) ; 
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psi_prime=psi_prime/tauf ; 

cl_prime=cl . * [5 1 : 0] *eye (6,5) ; 

lam_prime=polyval (cl_prime, taubar ) ; 
lam_prime=lam_prime/tauf ; 

%%  Computing  the  states  and  controls  using  ^Inverse  Dynamics* 

del_tau  =  tauf/ (NN— 1) ; 
tau  ( 1 )  =  0 ; 

time ( 1 )  =  0 ; 

Psi  =  atan2 (x_prime, y_prime) ;  %  Computing  heading,  rad 

Psi(l)  =  Psi  (2);  Psi (end) =Psi (end— 1) ; 
beta=Psi— psi; 

V(l)  =  lam ( 1 ) *sqrt (x_prime ( 1 ) A2+y_prime ( 1 ) A2 ) ;  %  Total  speed 

u(l)  =  V (1) *cos (beta (1) ) ;  %  surge  velocity 

v(l)  =  V ( 1 ) *sin (beta ( 1 ) ) ;  %  sway  velocity 

Psi_prime(l)  =  (x_prime ( 1 ) *y_dblprime ( 1 ) — y_prime ( 1 ) * . . . 

x_dblprime (1) ) / (y_prime (1) A2+x_prime (1) A2) ; 
Psi_dot(l)  =  lam ( 1 ) *Psi_prime ( 1 ) ; 
psi_dot(l)  =  lam ( 1 ) *psi_prime ( 1 ) ; 

for  j=2 : NN 

tau(j)  =  tau ( j— 1) +del_tau; 

dt  =  2*del_tau/ (lam ( j— 1) +lam ( j ) ) ; 

time(j)  =  time ( j— 1) +dt ; 

V ( j ) =  lam ( j ) *sqrt (x_prime ( j ) A2+y_prime ( j ) A2 ) ;  %  Total  speed 

u(j)  =  V ( j ) *cos (beta ( j ) ) ;  %  surge  velocity 

v(j)  =  V ( j ) *sin (beta ( j ) ) ;  %  sway  velocity 

Psi_prime ( j ) = (x_prime ( j ) *y_dblprime ( j ) — y_prime ( j ) * . . . 

x_dblprime ( j ) ) / (y_prime ( j ) A2+x_prime ( j ) A2 ) ; 
Psi_dot ( j ) =lam ( j ) *Psi_prime ( j ) ; 
psi_dot(j)  =  lam ( j ) *psi_prime ( j ) ; 

end 

Psi_dot  (end)  =Psi_dot  (end— 1)  ; 
beta_dot  =  Psi_dot  —  psi_dot; 

o, 

o 
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VARS^flam'  tau'  time'  x'  y’  u'  v*  V'  x_prime 1  y_prime '  beta'  beta_dot f . . . 

psi'  psi_dot '  Psi'  Psi_dotf]; 

%%Sonar  Imaging,  Probabilistic  Analysis,  Occupancy  Grid,  Obstacle  Avoidance 
if  ProbFlag 

%  Stuff  below  this  is  part  of  the  probabilistic  analysis  process. 

global  OG  Snrlmg  PrOpoly  PrEpoly 

global  L  IG  UB  BB  RB  LB  AP  RBx  BBy  ViolArea  avrad 
%%  Information  Gain  Analysis 
for  i=l:NN 

%  Perform  simulated  sonar  sweep 
sweep (psi (i) ,x(i) , y ( i) ) ; 

%  Weight  OG  values  at  heuristic  boundaries — this  reduces  the 
%  information  gain  quantified  by  objects  that  we  are  already 
%  assuming  to  be  there, 
if  i  ==  1 

OG ( 10*UB, 1 0  *  LB : 10* RB) =. 75; 

OG ( 10*UB : 10*BB, 10*RB) =. 75; 

end 

end 

%%  Update  Occupancy  Grid  and  Calculate  Information  Gain 

[ IGC1 ] =OGupdate (Snrlmg, PrOpoly, PrEpoly) ;  %  IGC1 :  IG  Calculation 

IG=IGC1 ; 

%%  Stern  Points  used  for  Heuristic  Checks 

%  Calculate  the  coordinates  of  the  stern  of  the  vehicle  at  each  point 
%  along  the  trajectory  and  determine  if  the  stern  points  cross  the 
%  heuristics  boundaries. 

for  i=l : NN 

xS (i) =x (i)— L/2*sin (psi (i) ) ;  %  Calc  x— coord  of  stern  for  ... 

each  x_cg 
if  xS ( i )  >  RBx 
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xViol (i) =1; 

else 

xViol (i) =0; 

end 

yS  (i) =y (i)— L/2*cos (psi (i) ) ;  %  Calc  y— coord  of  stern  for  ... 

each  y_cg 
if  yS ( i )  <  BBy 
yViol (i) =1; 

else 

yViol (i) =0; 

end 

end 

%%  Heuristic  Violation  Area  Analysis 

%  Uses  the  vectors  of  stern  point  coordinates  determined  above  to 
%  calculate  the  total  violation  area  between  the  arc  swept  by  the 
%  vehicle  stern  and  the  heuristics  boundaries. 

ViolArea=HeurViolArea (xS ( : ) , yS ( : ) , xViol ( : ) , yViol ( : ) ) ; 

%* * ** *END  PROBABILISTIC  (INFORMATION  GAIN)  ANALYSIS  STUFF***************** 

%9r9r9r********************************************************************** 

%%  Allision  Avoidance 

%  Calculate  the  amount  of  penetration  of  an  ob ject/obstacle  within  a 
%  circle  centered  on  the  center  of  the  vehicle  with  a  give  avoidance 
%  radius  (defined  in  Main_opt.m  script) . 

AvoidancePenalty=zeros (NN, 1 ) ;  %  Initialize  vector  (for  speed) 
for  i=l:NN 

AvoidancePenalty (i) ^Avoidance (beta (i) , x (i) ,  y (i) , avrad) ; 

end 

%  Cumulative  avoidance  penalty  along  trajectory: 

AP=sum (AvoidancePenalty ) ; 

%%  Favoring  Explored  Areas 

%  Calculate  penalty  for  trajectorys  that  drive  the  vehicle  outside  of 
%  areas  already  swept  by  sonar  (i.e.  "explored  areas") .  This  biases  the 
%  resulting  trajectories  into  known,  or  "explored"  space, 
global  EV 
for  i=l:NN 
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[me (i) , ne (i) ] =xy2mn (x (i) , y (i) ) ;  %  Convert  x,y  coords  to  matrix  coords 
0Gcval=0G (round (10*me (i) ), round (10*ne (i) )) ;  %  Determine  OG  cell  value 


%  at  each  x,y 

%  Calculate  absolute  difference  between 


ev ( i ) =abs (OGcval— 0 . 5 )  ; 


%  OGVal  and  the  OG  value  for  an 
%  unexplored  cell  (0.5)  . 


end 


EV=sum (ev)  ; 

end 

%%  Calculate  Performance  Index 
PI  =  Perf ormancelndex; 
return 

function  PI=PerformanceIndex 

%%  This  function  computes  the  combined  performance  index 

%  This  function  performs  the  actual  calculations  of  the  weighted  penalties 
%  for  the  Cost  Function  and  Performance  Index.  The  output  is  the  value 
%  that  is  actually  being  optimized  within  the  fminsearch  function  by 
%  changing  the  values  of  the  variable  parameters . 

%-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-kiz-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k 

global  wu  wv  wb  wY  ProbFlag  TH  wT  time  Cost_T 
global  u  v  u_max  v_max  psi_dot  psi_dot_max 
global  Fine_u  Fine_v  Fine_YawRate 
global  Fine_beta  beta  beta_max 


Fine_beta 


Fine_v 


Fine_YawRate 


Fine_u 


Cost_T 


(time (end)— TH) /'2; 

max  (  [0,  (abs (psi_dot)— psi_dot_max) ]  )  A2; 

max  (  [0,  (abs (u)— u_max) ] ) A2; 

max  (  [0,  (abs (v)— v_max) ] ) A2; 

max ( [0, (abs (beta) — beta_max) ]  )  A2; 


if  ProbFlag 

global  Cost_IG  Cost_H  Cost_AP  Cost_EV  wG  wH  wA  wE 
global  ViolArea  AP  IG  EV 


Cost_EV 


Cost_H 


Cost_AP 


Cost_IG 


ViolArea; 

AP; 

1/IG; 

1/ (1+EV) ; 


end 
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if  ProbFlag 

PI  =  wG*Cost_IG+wH*Cost_H+wA*Cost_AP+wu*Fine_u+wv*Fine_v+wY* • • • 

Fin  e_Y  a wRa t  e + wE *  C  o  s  t_E  V+ wT  *  C  o  s  t_T ; 

else 

PI  =  wu*Fine_u+wv*Fine_v+wY*Fine_YawRate+wb*Fine_beta+wT*Cost_T; 

end 

return 


A.3  Reference  Function  Generation  Function:  RefFuncs.m 


%  This  script  computes  coefficients  of  the  reference  polynomials 
%%  (x,y)  Reference  Function  (scaled  tau_bar=tau/tauf ) 
global  a  apsi  alam  Mp  Np  01 
syms  tf  xO  xpO  xppO  xf  xpf  xppf 


[1 

0 

0 

0 

0 

0; 

0 

1 

0 

0 

Pi 

2  *pi ; 

0 

0 

2 

0 

0 

0; 

1 

1 

1 

1 

0 

0; 

0 

1 

2 

3 

-pi 

2  *pi ; 

0 

0 

2 

6 

0 

0]  ; 

b=  [xO  (xp0*tf)  (xppO*tfA2)  xf  (xpf*tf)  (xppf *tf ^2) ] . ' ; 
a=A\b; 

a=collect (a,  tf )  ; 

Mp=length (a) ; 


%%  (psi)  Reference  Function  (scaled  tau_bar=tau/tauf ) 
syms  psiO  psipO  psippO  psiff  psipf  psippff  tf 


C=[l  0  0 

0  10 

0  0  2 

111 
0  12 

0  0  2 


0  0  0; 

0  0  0; 

0  0  0; 

1  11; 

3  4  5; 

6  12  20] ; 


d=[psi0  psip0*tf  psipp0*tf~2  psiff  psipf*tf  psippf f *tf A2 ] . ' ; 
apsi=C\d; 

apsi=collect (apsi,tf)  ; 

Np=length  (apsi) ; 


%%  (speed  profile)  Reference  Function\ 
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syms  lamO  lampO  lamppO  lamff  lampff  lamppff  tf 
LL= [  100000; 

0  1  0  0  0  0; 

0  0  2  0  0  0; 

111111; 

0  1  2  3  4  5; 

0  0  2  6  12  20] ; 

l=[lamO  lampO*tf  lamppO*tfA2  lamff  lampff*tf  lamppf f *tf A2 ] . 1 ; 
alam=LL\l ; 

alam=collect (alam, tf) ; 

01=length (alam) ; 

A.4  Sensor  Model  Generator  Function:  SensorModelGeneration.m 

%  Sonar  Sensor  Model: 

%  This  function  creates  the  probability  density  functions  for  the  sonar 
%  sensor  model.  It  takes  the  pre— fit  curve  fits  stored  in  this  folder 
%  (created  using  the  cfit  Matlab  toolbox)  and  creates  the  polynomials 
%  representing  each  sonar  sensor  probability  model  pdf  for  occupied/empty 
%  states.  The  empirical  data  input  in  the  sensor  probabilities  cells  (rO 
%  and  rE)  represents  the  data  used  to  create  the  pdf's  and  resulting  curve 
%  fits.  This  data  is  not  actually  used  during  runtime  due  to  the  fact  that 
%  it  is  already  incorporated  into  the  saved  curve  fits  PrOmdlfunc  and 
%  PrEmdlfunc. 

%  The  pdf's  may  be  plotted  by  uncommenting  the  plotting  code  in  the  last 
%  cell. 

global  PrOpoly  PrEpoly 

%  Signal  strengths  (x— axis) 

SS=linspace (0, 5000) ; 

SS=SS ' ; 

%%  Sensor  Probabilities  (Occupied) 

rO  =  [  0; 

0.5000; 

1.0000; 
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44 
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47 

12.0000 

48 

12.5000 

49 

13.0000 

50 
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51 

14.0000 

52 
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54 
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55 

16.0000 

56 

16.5000 

57 

17.0000 

58 

17.5000 

59 

18.0000 

60 

18.5000 

61 

19.0000 

62 

19.5000 

63 

20.0000 

64 

20.5000 

65 

21 . 1000 

66 

21 . 8000 

67 

22 . 6000 

68 

23.5000 

69 

24.5000 

70 

25.6000 

71 

26.8000 

72 

28.1000 

73 

29.5000 

74 

31.0000 

75 

32 . 6000 

76 

34.3000 

77 

36.1000 

78 

38.0000 

79 

40.0000 

80 

42 . 1000 

81 

44.3000 

82 

46.6000 

83 

49.0000 

84 

51.5000 

85 

53.9500 

86 

56.3500 

87 

58.7000 

88 

61.0000 

89 

63.2500 

90 

65.4500 

91 

67 . 6000 

92 

69.7000 

93 

71.7500 

94 

73.7500 

95 

75.7000 

96 

77 . 6000 
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79.4500 
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101 

86.3500 

102 
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127 

128 
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96.5000; 

97.7500; 

98.9500; 

100.1000; 

101.2000; 

102.2500; 

103.2500; 

104.2000; 

105.1000; 

105.9500; 

106.7500; 

107.5000; 

108.2000; 

108.8500; 

109.4500] ; 

%  Normalize  rO  Vector  (ensure  sum(rO)  =  1) 
rO=rO . / sum ( rO ) ; 

%  determine  function  representing  this  pdf 
load  PrOmdlfunc; 

PrOpdf=PrOmdlfunc; 

PrOpoly=coef fvalues (PrOpdf ) ; 

PrO=polyval (PrOpoly,  SS)  ; 

%  %  Plot  pdf  against  original  normalized  sensor  values 
%  plot (SS, PrO, ' : b ' ) 

%  hold  off 


%  %  Plot  error  between  rO  and  PrO 
%  Err=PrO— rO; 

%  plot (SS, Err) 

%%  Sensor  Probabilities  (Empty) 


%  rE=normpdf (SS, 500,500); 


rE= [ 25 ; 34 ; 54 ; 62;67;71;73;74;74;73;71;65;57;51;45;40;35;31;27;24;21;18;16; 
14;  12;  11;  10;  9;  8;  8;  7;  7;  6;  6;  5;  5;  5;  5;  4;  4;  4;  4;  4;  4;  3;  3;  3;  3;  3;  3;  3;  3;  3;  3;  3;  . 
2 ;  2 ;  2 ;  2 ;  2 ;  2 ;  2 ;  2 ;  2 ;  2 ;  2 ;  2 ;  2 ;  2 ;  2 ;  2 ;  2 ;  2 ;  2 ;  2 ;  2 ;  1 ;  1 ;  1 ;  1 ;  1 ;  1 ;  1 ;  1 ;  1 ;  1 ;  1 ;  1 ;  .  .  . 
i ;  i ;  i ;  i ;  i ;  i ;  i ;  i ;  i ;  i ;  i ;  i  ] ; 
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%  Normalize  rE  Vector  (ensure  sum(rE)  = 
rE=rE . / sum ( rE ) ; 

%  determine  function  representing  this  pdf 
load  PrEmdlfunc2; 

PrEpdf=PrEmdlfunc2 ; 

PrEpoly=coef fvalues (PrEpdf ) ; 

PrE=polyval (PrEpoly, SS)  ; 


%  PrE=PrE . /sum (PrE)  ; 

%  %  Plot  pdf  against  original  normalized  sensor  values 
%  plot (SS, PrE, 1 : b ’ ) 

%  hold  off 

%%  Plot  both  pdf's  together 
%  z  =  zeros  ( 100 , 1 ) ; 

%  figure ( ) 

%  plot  ( SS , PrE,  '  :r' , SS,PrO,  ' — b' , SS, z,  'k' ) 

%  title  ('Sonar  Return  Signal  Probability  Density  Functions') 

%  xlabel (' Return  Signal  Value') 

%  ylabel (' Probability  of  Receiving  Signal  Value') 

%  legend  (' P  [r_t_+_l  |  s (C) =Emp] ' , ' P [r_t_+_l  |  s(C)=Occ]') 

A.5  Sonar  Sweep  and  Imaging  Function:  sweep.m 

function  sweep (  hdg,xv,yv  ) 

%SWEEP  Performs  a  sonar  sweep  simulation  given  known  input  mapping  data 
%  This  function  takes  vehicle  state  inputs  and  a  known  world  model  of  the 
%  environment  to  determine  signal  strength  values  for  occupied/unoccupied 
%  pixels  in  the  sonar  sweep.  It  then  stores  these  signal  strength  values 
%  in  a  sonar  image  model  matrix  representing  a  notional  qualitative  sonar 
%  image  to  be  used  for  updating  the  Occupancy  Grid. 

%  Inputs: 

Vehicle  heading  (psi)  in  NED  coordinate  frame 
X  coordinate  of  vehicle  centroid 
Y  coordinate  of  vehicle  centroid 


hdg : 
xv : 
yv: 
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global  Snrlmg  WorldMdl  L  H  R  Rs 

alpha  =  45*pi/180;  %  Sonar  spread  half— width  in  radians 


%*****Next  four  values  are  arbitrarily  set.  Once  method  is  validated,  this 
%  might  need  to  be  changed  so  that  the  signal  returns  are  more  . . . 

randomized 

%  based  on  sonar  sensor  model. ***** 

sigOccMed  =  4000;  %  Median  signal  strength  to  use  for  occupied  cells 

sigOccDev  =  750;  %  Max  deviation  from  occupied  median  signal  strength 

sigEmpMed  =  300;  %  Median  signal  strength  to  use  for  empty  cells 

sigEmpDev  =  250;  %  Max  deviation  from  empty  median  signal  strength 


%******Change  the  next  line  to  refine  sweep*************** 
ang  =  l*pi/180;  %  Angle  increment  in  radians  * 

%********************************************************* 

xs  =  xv  +  L/2*sin (hdg) ;  %  Sonar  transmitter  x— coordinate  in  tank  frame 

ys  =  yv  +  L/2*cos (hdg) ;  %  Sonar  transmitter  y— coordinate  in  tank  frame 

%  Note:  following  are  scaled  by  10  to  change  into  the  matrix  space,  vice 
%  the  cartesian  space 

ms  =  round ( 10* (R+ (H—ys ))) ;  %  Map  matrix  row  index  of  ys 

ns  =  round (10* (R+xs) ) ;  %  Map  matrix  column  index  of  xs 

%  This  ’for'  loop  iterates  through  sonar  spread  in  angular  increment 
%  specified  by  variable  'ang'.  At  each  step,  it  generates  a  Bresenham 
%  line,  which  it  then  compares  to  the  known  image  map  ('WorldMdl')  to  ... 
assign 

%  a  value  of  1  or  0  to  each  point  on  the  Bresenham  line  corresponding  to 
%  the  value  for  that  point  within  the  image  map. 

for  a=hdg— alpha : ang : hdg+alpha 

mr=ms— round (Rs*cos (a) ) ;  %  row  index  of  Bresenham  line  endpoint  ... 

y— coord 

nr=ns+round (Rs*sin (a) ) ;  %  column  index  of  Bresenham  line  endpoint  ... 

x— coord 

%  Bresenham  line  creation 

[bn  bm]  =  bresenham (ns, ms, nr, mr) ; 
len  =  length  (bn); 
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bv=zeros (1, len) ; 
for  j=l:len 

bv ( j ) =WorldMdl (bm ( j ) , bn ( j ) ) ; 
point  on 


%  Assign  1  or  0  value  to  each  . . . 

%  Bresenham  line  corresponding  . . . 
to  those 

%  points  in  input  binary  matrix 


%  This  next  part  chooses  random  values  between  min  empty/occupied 
%  signal  and  max  empty/occupied  signal  as  defined  above.  This 
%  randomizes  the  signal  return  value,  adding  realism  to  the 
%  simulation, 
if  bv(j)  ==  0 

sigEmpmin=sigEmpMed— sigEmpDev; 
sigEmpmax=sigEmpMed+sigEmpDev; 
sigEmp=randi ( [sigEmpmin  sigEmpmax] ) ; 

Snrlmg (bm ( j ), bn ( j ) )  =  sigEmp; 
elseif  bv(j)  ==  1 

sigOccmin=sigOccMed— sigOccDev; 
sigOccmax=sigOccMed+sigOccDev; 
sigOcc=randi ( [sigOccmin  sigOccmax] ) ; 

Snrlmg (bm ( j ), bn ( j ) )  =  sigOcc; 
break 

end 

end 


end 

end 

A.6  Occupancy  Grid  Updater  and  Information  Gain  Calcu¬ 
lation  Function:  OGupdate.m 

function  [IG1  ] =OGupdate (  snrimg, occpdf , emppdf  ) 

%0GUPDATE  Updates  the  probabilities  in  Occupancy  Grid  for  each  sonar  sweep 
%and  calculates  the  resulting  information  gain. 

%  This  function  takes  the  sonar  image  developed  in  the  sweep  function  as 
%  well  as  the  sensor  signal  probability  density  functions,  and 
%  updates  the  Occupancy  Grid  usign  the  Bayesian  Inference  equation.  It 
%  then  calculates  the  information  gain  acheived  during  each  cell  update. 
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%  and  finally  calculates  the  cumulative  information  gain  over  the  entire 
%  occupancy  grid  for  the  given  trajectory. 

global  OG  %HE 

[r  c] =size ( snrimg) ; 

igl=zeros ( r , c) ;  %  Matrix  preallocation  for  speed 
%  ig2=zeros (r , c) ; 
for  i=l : r 

for  j=l:c 

if  snrimg (i, j )  ^  0 

PsO  =  polyval (occpdf, snrimg (i, j )) ; 

PsE  =  polyval (emppdf, snrimg ( i ,  j )) ; 
elseif  snrimg(i, j)  ==  0 
PsE=0 . 5 ; 

PsO=0 . 5 ; 

end 

PcOt=OG (i, j ) ;  %  Prior  OG  cell  value 

%  Bayesian  inference  formula  for  posterior  OG  cell  value: 

PcO= (PsO*PcOt ) / (PsE* (1-PcOt) +PsO*PcOt)  / 

%  Simple  IG  method 
igl (i, j ) =abs (PcO— PcOt ) ; 

%  Alternate  IG  method:  This  method  was  derived  from  the 
%  Probabilistic  Robotics  book.  However,  it  proved  to  be  very 
%  computationally  intensive,  and  produced  results  very  close  to  the 
%  much  simpler  method  actually  employed  above,  thus  it  is  not  used 
%  at  this  time. 

%  Update  Entropy 

%  Pi=OG(i,j);  %  Prior  cell  occupancy  probability 
%  Ptrue=PcO;  %  Probability  for  correctly  measuring  . . . 
cell  is  Occupied 

%  Hp=— Pi*log2 (Pi)—  (1— Pi) *log2 (1— Pi) ;  %  Prior  cell  ... 

entropy 

%  HE(i,j)=Hp;  %  Update  entropy  grid 

%  Expected  entropy  after  sensing  (Probabilistic  Robotics, 

%  Eqn.  17.15,  pg .  584) 
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o, 
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o, 

o 


o, 

o 


o. 

o 


EH=— Ptrue*Pi*log2 (Ptrue*Pi/ (Ptrue*Pi+ (1— Ptrue) * (1— Pi) ) ) . . . 

—  (1— Ptrue) * (1— Pi) *log2 ( (1— Ptrue) * (1— Pi) / (Ptrue*Pi+ (1— Ptrue) * (1— Pi) ) )  .  .  . 

—  (1— Ptrue) *Pi*log2 ( (1— Ptrue) *Pi/ (Ptrue* (1— Pi)  +  (1— Ptrue) *Pi) )  .  . . 

—Ptrue* (1— Pi) *log2 ( (1— Ptrue) *Pi/  (Ptrue* (1— Pi)  +  (1— Ptrue) *Pi) ) ; 

ig2 (i, j ) =Hp— EH;  %  Expected  Information  Gain  (Probabilistic  ... 

Robotics , 


%  Eqn.  17.4,  pg .  572) 


%  Update  Occupancy  Grid 
OG (i, j) =PcO; 


end 

end 

IGl=sum ( sum ( igl ) ) ;  %  Calculate  total  IG  over  entire  OG 

%  IG2=sum (sum (ig2) ) ; 

A.7  Heuristics  Violation  Analysis  Function:  HeurViolArea.m 

function  [  ViolArea  ]  =  HeurViolArea (  xS, yS, xViol, yViol  ) 

%HEURVIOLAREA — calculates  heuristics  boundary  violation  areas 

%  This  function  uses  the  vectors  of  stern  point  coordinates  to 
%  calculate  the  total  violation  area  between  the  arc  swept  by  the 
%  vehicle  stern  and  the  heuristics  boundaries. 

global  RBx  BBy 

if  (any (xViol))  &&  (any (yViol)) 

ViolArea=NaN; 

elseif  (any (xViol))  | |  (any (yViol)) 
if  any (xViol) 

%  fprintf('Run  %g:  Violation  of  Right  Boundary . \n n) 
xViolVec=xS (find (xViol ) )  ; 
xvl=length (xViolVec)  ; 
for  i=l : xvl 
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xV (i) =xViolVec (i)— RBx; 

end 

%  ★★★Calculate  Violation  Area 
xVint=trapz (yS (find (xViol) ) , xV) ; 

ViolArea=abs (xVint) ; 

%  fprintf (’ Violation  of  Right  Boundary:  %6 . 4g\n ' , ViolArea) 

%  ★  ★  ★ 

%  ★★★Plot  right  boundary  violation  area — use  this  for  a  visual  plot 
%  of  the  actual  boundary  violations. 

RBxp=ones ( 1 , xvl) *RBx; 
xviolf ig=f igure  () ; 

plot (yS (find (xViol ) ) , xViolVec, ’ k ' , yS (find (xViol ) ) , RBxp, ' r* ' ) ; 
jbf ill (yS (find (xViol ) ) , RBxp, xViolVec) ; 
axis  equal 

t st r=sprint f ( ' Run  %g  Right  Boundary  Violation  Plot*,n); 
title  (tstr) 
xlabel ( ' y ' ) 
ylabel ( ' x 1 ) 

legend (' Stern  Path', 'Right  Boundary', 'Location', 'North') 
set  (gca,  ' YDir ' ,  ' reverse ' ) 
camroll ( 90 ) 

%  ★  ★  ★ 

end 

if  any(yViol) 

%  fprintf ('Run  %g:  Violation  of  Bottom  Boundary . \n ', n) 
yViolVec=yS (find (yViol) ) ; 
yvl=length (yViolVec) ; 

% [myv  nyv] =size (yViolVec)  %  t/s  only 
for  i=l:yvl 

yV  (i) =BBy— yViolVec (i)  ; 

end 

%  ★★★Calculate  Violation  Area 
yVint=trapz (xS (find (yViol) ) , yV) ; 

ViolArea=abs (yVint) ; 

%  fprintf (' Violation  of  Lower  Boundary:  %6 . 4g\n ', ViolArea) 

%  ★  ★  ★ 
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%  ***p]_ot  bottom  boundary  violation  area — use  this  for  a  visual  .  .  . 
plot 

%  of  the  actual  boundary  violations. 

%  BByp=ones ( 1 , yvl ) *BBy ; 

%  [mby  nby ] =size (BByp)  %t/s  only 

%  yviolf ig=f igure  ( ) ; 

%  plot (xS (find (yViol) ) , yViolVec,  ' k ’ , xS (find (yViol) ) , BByp,  f  r* ' ) 

%  jbfill(xS (find (yViol) ) , BByp, yViolVec) ; 

%  axis  equal 

%  t str=sprint f ( ' Run  %g  Bottom  Boundary  Violation  Plot’ ,n) ; 

%  title (tstr) 

%  xlabel ( ' x ' ) 

%  ylabel(’y') 

%  legend (' Stern  Path Bottom  Boundary Location Best ' ) 

%  k  -k  -k 

end 

else  ViolArea  =  0; 
end 

end 


A.8  Avoidance  Penalty  Analysis  Function:  Avoidance. m 

%  This  script  generates  the  World  Model  matrix  and  initializes  the  matrices 
%  for  the  sonar  image,  heuristics  analysis,  occupancy  grid,  and  information 
%  gain  value  storage. 

%  World  Model  (Input  data  for  simulation) 

global  HMat  OG  Snrlmg  WorldMdl  R  M  N  IG1  ObsFlag  Rs  H  xl  xr  yl  yu  xl2  . . . 
xr2  yl2  yu2  %HE 

%  Tank  matrix  for  Bresenham  imaging  algorithm 

Rs=10*R;  %  Scales  the  sonar  range  value  for  matrix  cell 

%  index  addressing. 

WorldMdl=zeros (M,N) ; 

%  The  following  4  'for'  loops  place  ones  in  tank  matrix  space  outside  . . . 
the  tank 
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for  i=l : Rs 

WorldMdl (i, : ) =1 ; 

end 

for  j=M— Rs:M 

WorldMdl ( j , : ) =  1 ; 

end 

for  k=l : Rs 

WorldMdl ( : ,  k) =1; 

end 

for  1=N— Rs : N 

WorldMdl ( : , 1) =1 ; 

end 

if  ObsFlag 
%  Obstacles  in  tank: 
ml=10* (R+H— yu) ; 
m2=10* (R+H— yl) ; 
nl=10* (xl+R)  ; 
n2=10* (xr+R)  ; 

WorldMdl (ml : m2 ,  nl : n2 ) =1 ;  %  Assigns  value  of  one  to  all  World  Model 

%  cells  corresponding  to  the  obstacle  location. 


ml2=10* (R+H— yu2) ; 
m22=10* (R+H— yl2 ) ; 
nl2=10* (xl2+R)  ; 
n22  =  10* (xr2+R)  ; 

WorldMdl (ml 2 : m2 2 ,  nl2 : n22 ) =1 ; 

end 


SnrImg=zeros (M,N) ; 
HMat=WorldMdl ; 

OG= . 5* ones (M, N) ; 
IGl=zeros (M,N) ; 


%  Initialize  Sonar  Image  matrix 
%  Used  for  Heuristic  Violation  Analysis 
%  Initialize  Occupancy  Grid 
%  Initialize  Information  Gain  matrix 
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